diff --git a/src/HAL/AVR/HAL.cpp b/src/HAL/AVR/HAL.cpp new file mode 100644 index 0000000..5382eb3 --- /dev/null +++ b/src/HAL/AVR/HAL.cpp @@ -0,0 +1,172 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __AVR__ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" +#include + +#ifdef USBCON + DefaultSerial1 MSerial0(false, Serial); + #ifdef BLUETOOTH + BTSerial btSerial(false, bluetoothSerial); + #endif +#endif + +// ------------------------ +// Public Variables +// ------------------------ + +// Don't initialize/override variable (which would happen in .init4) +uint8_t MarlinHAL::reset_reason __attribute__((section(".noinit"))); + +// ------------------------ +// Public functions +// ------------------------ + +__attribute__((naked)) // Don't output function pro- and epilogue +__attribute__((used)) // Output the function, even if "not used" +__attribute__((section(".init3"))) // Put in an early user definable section +void save_reset_reason() { + #if ENABLED(OPTIBOOT_RESET_REASON) + __asm__ __volatile__( + A("STS %0, r2") + : "=m"(hal.reset_reason) + ); + #else + hal.reset_reason = MCUSR; + #endif + + // Clear within 16ms since WDRF bit enables a 16ms watchdog timer -> Boot loop + hal.clear_reset_source(); + wdt_disable(); +} + +void MarlinHAL::init() { + // Init Servo Pins + #define INIT_SERVO(N) OUT_WRITE(SERVO##N##_PIN, LOW) + #if HAS_SERVO_0 + INIT_SERVO(0); + #endif + #if HAS_SERVO_1 + INIT_SERVO(1); + #endif + #if HAS_SERVO_2 + INIT_SERVO(2); + #endif + #if HAS_SERVO_3 + INIT_SERVO(3); + #endif + + init_pwm_timers(); // Init user timers to default frequency - 1000HZ +} + +void MarlinHAL::reboot() { + #if ENABLED(USE_WATCHDOG) + while (1) { /* run out the watchdog */ } + #else + void (*resetFunc)() = 0; // Declare resetFunc() at address 0 + resetFunc(); // Jump to address 0 + #endif +} + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #include + #include "../../MarlinCore.h" + + // Initialize watchdog with 8s timeout, if possible. Otherwise, make it 4s. + void MarlinHAL::watchdog_init() { + #if ENABLED(WATCHDOG_DURATION_8S) && defined(WDTO_8S) + #define WDTO_NS WDTO_8S + #else + #define WDTO_NS WDTO_4S + #endif + #if ENABLED(WATCHDOG_RESET_MANUAL) + // Enable the watchdog timer, but only for the interrupt. + // Take care, as this requires the correct order of operation, with interrupts disabled. + // See the datasheet of any AVR chip for details. + wdt_reset(); + cli(); + _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE); + _WD_CONTROL_REG = _BV(WDIE) | (WDTO_NS & 0x07) | ((WDTO_NS & 0x08) << 2); // WDTO_NS directly does not work. bit 0-2 are consecutive in the register but the highest value bit is at bit 5 + // So worked for up to WDTO_2S + sei(); + wdt_reset(); + #else + wdt_enable(WDTO_NS); // The function handles the upper bit correct. + #endif + //delay(10000); // test it! + } + + //=========================================================================== + //=================================== ISR =================================== + //=========================================================================== + + // Watchdog timer interrupt, called if main program blocks >4sec and manual reset is enabled. + #if ENABLED(WATCHDOG_RESET_MANUAL) + ISR(WDT_vect) { + sei(); // With the interrupt driven serial we need to allow interrupts. + SERIAL_ERROR_MSG(STR_WATCHDOG_FIRED); + minkill(); // interrupt-safe final kill and infinite loop + } + #endif + + // Reset watchdog. MUST be called at least every 4 seconds after the + // first watchdog_init or AVR will go into emergency procedures. + void MarlinHAL::watchdog_refresh() { wdt_reset(); } + +#endif // USE_WATCHDOG + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#if ENABLED(SDSUPPORT) + + #include "../../sd/SdFatUtil.h" + int freeMemory() { return SdFatUtil::FreeRam(); } + +#else // !SDSUPPORT + + extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + int freeMemory() { + int free_memory; + if ((int)__brkval == 0) + free_memory = ((int)&free_memory) - ((int)&__bss_end); + else + free_memory = ((int)&free_memory) - ((int)__brkval); + return free_memory; + } + } + +#endif // !SDSUPPORT + +#endif // __AVR__ diff --git a/src/HAL/AVR/HAL.h b/src/HAL/AVR/HAL.h new file mode 100644 index 0000000..1491867 --- /dev/null +++ b/src/HAL/AVR/HAL.h @@ -0,0 +1,277 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Arduino AVR + */ + +#include "../shared/Marduino.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "math.h" + +#ifdef USBCON + #include +#else + #include "MarlinSerial.h" +#endif + +#include +#include +#include +#include +#include +#include + +// +// Default graphical display delays +// +#if F_CPU >= 20000000 + #define CPU_ST7920_DELAY_1 150 + #define CPU_ST7920_DELAY_2 0 + #define CPU_ST7920_DELAY_3 150 +#elif F_CPU == 16000000 + #define CPU_ST7920_DELAY_1 125 + #define CPU_ST7920_DELAY_2 0 + #define CPU_ST7920_DELAY_3 188 +#endif + +#ifndef pgm_read_ptr + // Compatibility for avr-libc 1.8.0-4.1 included with Ubuntu for + // Windows Subsystem for Linux on Windows 10 as of 10/18/2019 + #define pgm_read_ptr_far(address_long) (void*)__ELPM_word((uint32_t)(address_long)) + #define pgm_read_ptr_near(address_short) (void*)__LPM_word((uint16_t)(address_short)) + #define pgm_read_ptr(address_short) pgm_read_ptr_near(address_short) +#endif + +// ------------------------ +// Defines +// ------------------------ + +// AVR PROGMEM extension for sprintf_P +#define S_FMT "%S" + +// AVR PROGMEM extension for string define +#define PGMSTR(NAM,STR) const char NAM[] PROGMEM = STR + +#ifndef CRITICAL_SECTION_START + #define CRITICAL_SECTION_START() unsigned char _sreg = SREG; cli() + #define CRITICAL_SECTION_END() SREG = _sreg +#endif + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment +#define PWM_FREQUENCY 1000 // Default PWM frequency when set_pwm_duty() is called without set_pwm_frequency() + +// ------------------------ +// Types +// ------------------------ + +typedef int8_t pin_t; + +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; + +// ------------------------ +// Serial ports +// ------------------------ + +#ifdef USBCON + #include "../../core/serial_hook.h" + typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; + extern DefaultSerial1 MSerial0; + #ifdef BLUETOOTH + typedef ForwardSerial1Class< decltype(bluetoothSerial) > BTSerial; + extern BTSerial btSerial; + #endif + + #define MYSERIAL1 TERN(BLUETOOTH, btSerial, MSerial0) +#else + #if !WITHIN(SERIAL_PORT, -1, 3) + #error "SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." + #endif + #define MYSERIAL1 customizedSerial1 + + #ifdef SERIAL_PORT_2 + #if !WITHIN(SERIAL_PORT_2, -1, 3) + #error "SERIAL_PORT_2 must be from 0 to 3, or -1 for USB Serial." + #endif + #define MYSERIAL2 customizedSerial2 + #endif + + #ifdef SERIAL_PORT_3 + #if !WITHIN(SERIAL_PORT_3, -1, 3) + #error "SERIAL_PORT_3 must be from 0 to 3, or -1 for USB Serial." + #endif + #define MYSERIAL3 customizedSerial3 + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if !WITHIN(MMU2_SERIAL_PORT, -1, 3) + #error "MMU2_SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." + #endif + #define MMU2_SERIAL mmuSerial +#endif + +#ifdef LCD_SERIAL_PORT + #if !WITHIN(LCD_SERIAL_PORT, -1, 3) + #error "LCD_SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." + #endif + #define LCD_SERIAL lcdSerial + #if HAS_DGUS_LCD + #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.get_tx_buffer_free() + #endif +#endif + +// +// ADC +// +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +#define HAL_SENSITIVE_PINS 0, 1, + +#ifdef __AVR_AT90USB1286__ + #define JTAG_DISABLE() do{ MCUCR = 0x80; MCUCR = 0x80; }while(0) +#endif + +// AVR compatibility +#define strtof strtod + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +extern "C" int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return TEST(SREG, SREG_I); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t reset_reason; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() { MCUSR = 0; } + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + // Called by Temperature::init once at startup + static void adc_init() { + ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07; + DIDR0 = 0; + #ifdef DIDR2 + DIDR2 = 0; + #endif + } + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) { + #ifdef DIDR2 + if (ch > 7) { SBI(DIDR2, ch & 0x07); return; } + #endif + SBI(DIDR0, ch); + } + + // Begin ADC sampling on the given channel. Called from Temperature::isr! + static void adc_start(const uint8_t ch) { + #ifdef MUX5 + ADCSRB = ch > 7 ? _BV(MUX5) : 0; + #else + ADCSRB = 0; + #endif + ADMUX = _BV(REFS0) | (ch & 0x07); + SBI(ADCSRA, ADSC); + } + + // Is the ADC ready for reading? + static bool adc_ready() { return !TEST(ADCSRA, ADSC); } + + // The current value of the ADC register + static __typeof__(ADC) adc_value() { return ADC; } + + /** + * init_pwm_timers + * Set the default frequency for timers 2-5 to 1000HZ + */ + static void init_pwm_timers(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Set the frequency of the timer for the given pin as close as + * possible to the provided desired frequency. Internally calculate + * the required waveform generation mode, prescaler, and resolution + * values and set timer registers accordingly. + * NOTE that the frequency is applied to all pins on the timer (Ex OC3A, OC3B and OC3B) + * NOTE that there are limitations, particularly if using TIMER2. (see Configuration_adv.h -> FAST_PWM_FAN Settings) + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); +}; diff --git a/src/HAL/AVR/HAL_SPI.cpp b/src/HAL/AVR/HAL_SPI.cpp new file mode 100644 index 0000000..dc98f2f --- /dev/null +++ b/src/HAL/AVR/HAL_SPI.cpp @@ -0,0 +1,254 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Adapted from Arduino Sd2Card Library + * Copyright (c) 2009 by William Greiman + */ + +/** + * HAL for AVR - SPI functions + */ + +#ifdef __AVR__ + +#include "../../inc/MarlinConfig.h" + +void spiBegin() { + #if PIN_EXISTS(SD_SS) + // Do not init HIGH for boards with pin 4 used as Fans or Heaters or otherwise, not likely to have multiple SPI devices anyway. + #if defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__) + // SS must be in output mode even it is not chip select + SET_OUTPUT(SD_SS_PIN); + #else + // set SS high - may be chip select for another SPI device + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + #endif + SET_OUTPUT(SD_SCK_PIN); + SET_INPUT(SD_MISO_PIN); + SET_OUTPUT(SD_MOSI_PIN); + + IF_DISABLED(SOFTWARE_SPI, spiInit(SPI_HALF_SPEED)); +} + +#if NONE(SOFTWARE_SPI, FORCE_SOFT_SPI) + + // ------------------------ + // Hardware SPI + // ------------------------ + + // make sure SPCR rate is in expected bits + #if (SPR0 != 0 || SPR1 != 1) + #error "unexpected SPCR bits" + #endif + + /** + * Initialize hardware SPI + * Set SCK rate to F_CPU/pow(2, 1 + spiRate) for spiRate [0,6] + */ + void spiInit(uint8_t spiRate) { + // See avr processor documentation + CBI( + #ifdef PRR + PRR + #elif defined(PRR0) + PRR0 + #endif + , PRSPI + ); + + SPCR = _BV(SPE) | _BV(MSTR) | (spiRate >> 1); + SPSR = spiRate & 1 || spiRate == 6 ? 0 : _BV(SPI2X); + } + + /** SPI receive a byte */ + uint8_t spiRec() { + SPDR = 0xFF; + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + return SPDR; + } + + /** SPI read data */ + void spiRead(uint8_t *buf, uint16_t nbyte) { + if (nbyte-- == 0) return; + SPDR = 0xFF; + for (uint16_t i = 0; i < nbyte; i++) { + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + buf[i] = SPDR; + SPDR = 0xFF; + } + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + buf[nbyte] = SPDR; + } + + /** SPI send a byte */ + void spiSend(uint8_t b) { + SPDR = b; + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + } + + /** SPI send block */ + void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPDR = token; + for (uint16_t i = 0; i < 512; i += 2) { + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + SPDR = buf[i]; + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + SPDR = buf[i + 1]; + } + while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + } + + + /** begin spi transaction */ + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + // Based on Arduino SPI library + // Clock settings are defined as follows. Note that this shows SPI2X + // inverted, so the bits form increasing numbers. Also note that + // fosc/64 appears twice + // SPR1 SPR0 ~SPI2X Freq + // 0 0 0 fosc/2 + // 0 0 1 fosc/4 + // 0 1 0 fosc/8 + // 0 1 1 fosc/16 + // 1 0 0 fosc/32 + // 1 0 1 fosc/64 + // 1 1 0 fosc/64 + // 1 1 1 fosc/128 + + // We find the fastest clock that is less than or equal to the + // given clock rate. The clock divider that results in clock_setting + // is 2 ^^ (clock_div + 1). If nothing is slow enough, we'll use the + // slowest (128 == 2 ^^ 7, so clock_div = 6). + uint8_t clockDiv; + + // When the clock is known at compiletime, use this if-then-else + // cascade, which the compiler knows how to completely optimize + // away. When clock is not known, use a loop instead, which generates + // shorter code. + if (__builtin_constant_p(spiClock)) { + if (spiClock >= F_CPU / 2) clockDiv = 0; + else if (spiClock >= F_CPU / 4) clockDiv = 1; + else if (spiClock >= F_CPU / 8) clockDiv = 2; + else if (spiClock >= F_CPU / 16) clockDiv = 3; + else if (spiClock >= F_CPU / 32) clockDiv = 4; + else if (spiClock >= F_CPU / 64) clockDiv = 5; + else clockDiv = 6; + } + else { + uint32_t clockSetting = F_CPU / 2; + clockDiv = 0; + while (clockDiv < 6 && spiClock < clockSetting) { + clockSetting /= 2; + clockDiv++; + } + } + + // Compensate for the duplicate fosc/64 + if (clockDiv == 6) clockDiv = 7; + + // Invert the SPI2X bit + clockDiv ^= 0x1; + + SPCR = _BV(SPE) | _BV(MSTR) | ((bitOrder == LSBFIRST) ? _BV(DORD) : 0) | + (dataMode << CPHA) | ((clockDiv >> 1) << SPR0); + SPSR = clockDiv | 0x01; + } + + +#else // SOFTWARE_SPI || FORCE_SOFT_SPI + + // ------------------------ + // Software SPI + // ------------------------ + + // nop to tune soft SPI timing + #define nop asm volatile ("\tnop\n") + + void spiInit(uint8_t) { /* do nothing */ } + + // Begin SPI transaction, set clock, bit order, data mode + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { /* do nothing */ } + + // Soft SPI receive byte + uint8_t spiRec() { + uint8_t data = 0; + // no interrupts during byte receive - about 8µs + cli(); + // output pin high - like sending 0xFF + WRITE(SD_MOSI_PIN, HIGH); + + LOOP_L_N(i, 8) { + WRITE(SD_SCK_PIN, HIGH); + + nop; // adjust so SCK is nice + nop; + + data <<= 1; + + if (READ(SD_MISO_PIN)) data |= 1; + + WRITE(SD_SCK_PIN, LOW); + } + + sei(); + return data; + } + + // Soft SPI read data + void spiRead(uint8_t *buf, uint16_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) + buf[i] = spiRec(); + } + + // Soft SPI send byte + void spiSend(uint8_t data) { + // no interrupts during byte send - about 8µs + cli(); + LOOP_L_N(i, 8) { + WRITE(SD_SCK_PIN, LOW); + WRITE(SD_MOSI_PIN, data & 0x80); + data <<= 1; + WRITE(SD_SCK_PIN, HIGH); + } + + nop; // hold SCK high for a few ns + nop; + nop; + nop; + + WRITE(SD_SCK_PIN, LOW); + + sei(); + } + + // Soft SPI send block + void spiSendBlock(uint8_t token, const uint8_t *buf) { + spiSend(token); + for (uint16_t i = 0; i < 512; i++) + spiSend(buf[i]); + } + +#endif // SOFTWARE_SPI || FORCE_SOFT_SPI + +#endif // __AVR__ diff --git a/src/HAL/AVR/MarlinSPI.h b/src/HAL/AVR/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/AVR/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/AVR/MarlinSerial.cpp b/src/HAL/AVR/MarlinSerial.cpp new file mode 100644 index 0000000..9864624 --- /dev/null +++ b/src/HAL/AVR/MarlinSerial.cpp @@ -0,0 +1,652 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * MarlinSerial.cpp - Hardware serial library for Wiring + * Copyright (c) 2006 Nicholas Zambetti. All right reserved. + * + * Modified 23 November 2006 by David A. Mellis + * Modified 28 September 2010 by Mark Sproul + * Modified 14 February 2016 by Andreas Hardtung (added tx buffer) + * Modified 01 October 2017 by Eduardo José Tagle (added XON/XOFF) + * Modified 10 June 2018 by Eduardo José Tagle (See #10991) + * Templatized 01 October 2018 by Eduardo José Tagle to allow multiple instances + */ + +#ifdef __AVR__ + +// Disable HardwareSerial.cpp to support chips without a UART (Attiny, etc.) + +#include "../../inc/MarlinConfig.h" + +#if !defined(USBCON) && (defined(UBRRH) || defined(UBRR0H) || defined(UBRR1H) || defined(UBRR2H) || defined(UBRR3H)) + +#include "MarlinSerial.h" +#include "../../MarlinCore.h" + +#if ENABLED(DIRECT_STEPPING) + #include "../../feature/direct_stepping.h" +#endif + +template typename MarlinSerial::ring_buffer_r MarlinSerial::rx_buffer = { 0, 0, { 0 } }; +template typename MarlinSerial::ring_buffer_t MarlinSerial::tx_buffer = { 0 }; +template bool MarlinSerial::_written = false; +template uint8_t MarlinSerial::xon_xoff_state = MarlinSerial::XON_XOFF_CHAR_SENT | MarlinSerial::XON_CHAR; +template uint8_t MarlinSerial::rx_dropped_bytes = 0; +template uint8_t MarlinSerial::rx_buffer_overruns = 0; +template uint8_t MarlinSerial::rx_framing_errors = 0; +template typename MarlinSerial::ring_buffer_pos_t MarlinSerial::rx_max_enqueued = 0; + +// A SW memory barrier, to ensure GCC does not overoptimize loops +#define sw_barrier() asm volatile("": : :"memory"); + +#include "../../feature/e_parser.h" + +// "Atomically" read the RX head index value without disabling interrupts: +// This MUST be called with RX interrupts enabled, and CAN'T be called +// from the RX ISR itself! +template +FORCE_INLINE typename MarlinSerial::ring_buffer_pos_t MarlinSerial::atomic_read_rx_head() { + if (Cfg::RX_SIZE > 256) { + // Keep reading until 2 consecutive reads return the same value, + // meaning there was no update in-between caused by an interrupt. + // This works because serial RX interrupts happen at a slower rate + // than successive reads of a variable, so 2 consecutive reads with + // the same value means no interrupt updated it. + ring_buffer_pos_t vold, vnew = rx_buffer.head; + sw_barrier(); + do { + vold = vnew; + vnew = rx_buffer.head; + sw_barrier(); + } while (vold != vnew); + return vnew; + } + else { + // With an 8bit index, reads are always atomic. No need for special handling + return rx_buffer.head; + } +} + +template +volatile bool MarlinSerial::rx_tail_value_not_stable = false; +template +volatile uint16_t MarlinSerial::rx_tail_value_backup = 0; + +// Set RX tail index, taking into account the RX ISR could interrupt +// the write to this variable in the middle - So a backup strategy +// is used to ensure reads of the correct values. +// -Must NOT be called from the RX ISR - +template +FORCE_INLINE void MarlinSerial::atomic_set_rx_tail(typename MarlinSerial::ring_buffer_pos_t value) { + if (Cfg::RX_SIZE > 256) { + // Store the new value in the backup + rx_tail_value_backup = value; + sw_barrier(); + // Flag we are about to change the true value + rx_tail_value_not_stable = true; + sw_barrier(); + // Store the new value + rx_buffer.tail = value; + sw_barrier(); + // Signal the new value is completely stored into the value + rx_tail_value_not_stable = false; + sw_barrier(); + } + else + rx_buffer.tail = value; +} + +// Get the RX tail index, taking into account the read could be +// interrupting in the middle of the update of that index value +// -Called from the RX ISR - +template +FORCE_INLINE typename MarlinSerial::ring_buffer_pos_t MarlinSerial::atomic_read_rx_tail() { + if (Cfg::RX_SIZE > 256) { + // If the true index is being modified, return the backup value + if (rx_tail_value_not_stable) return rx_tail_value_backup; + } + // The true index is stable, return it + return rx_buffer.tail; +} + +// (called with RX interrupts disabled) +template +FORCE_INLINE void MarlinSerial::store_rxd_char() { + + static EmergencyParser::State emergency_state; // = EP_RESET + + // This must read the R_UCSRA register before reading the received byte to detect error causes + if (Cfg::DROPPED_RX && B_DOR && !++rx_dropped_bytes) --rx_dropped_bytes; + if (Cfg::RX_OVERRUNS && B_DOR && !++rx_buffer_overruns) --rx_buffer_overruns; + if (Cfg::RX_FRAMING_ERRORS && B_FE && !++rx_framing_errors) --rx_framing_errors; + + // Read the character from the USART + uint8_t c = R_UDR; + + #if ENABLED(DIRECT_STEPPING) + if (page_manager.maybe_store_rxd_char(c)) return; + #endif + + // Get the tail - Nothing can alter its value while this ISR is executing, but there's + // a chance that this ISR interrupted the main process while it was updating the index. + // The backup mechanism ensures the correct value is always returned. + const ring_buffer_pos_t t = atomic_read_rx_tail(); + + // Get the head pointer - This ISR is the only one that modifies its value, so it's safe to read here + ring_buffer_pos_t h = rx_buffer.head; + + // Get the next element + ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the RX FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + + if (Cfg::MAX_RX_QUEUED) { + // Calculate count of bytes stored into the RX buffer + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Keep track of the maximum count of enqueued bytes + NOLESS(rx_max_enqueued, rx_count); + } + + if (Cfg::XONOFF) { + // If the last char that was sent was an XON + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) { + + // Bytes stored into the RX buffer + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // If over 12.5% of RX buffer capacity, send XOFF before running out of + // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react + // and stop sending bytes. This translates to 13mS propagation time. + if (rx_count >= (Cfg::RX_SIZE) / 8) { + + // At this point, definitely no TX interrupt was executing, since the TX ISR can't be preempted. + // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens + // to be in the middle of trying to disable the RX interrupt in the main program, eventually the + // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure + // the sending of the XOFF char is to send it HERE AND NOW. + + // About to send the XOFF char + xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT; + + // Wait until the TX register becomes empty and send it - Here there could be a problem + // - While waiting for the TX register to empty, the RX register could receive a new + // character. This must also handle that situation! + while (!B_UDRE) { + + if (B_RXC) { + // A char arrived while waiting for the TX buffer to be empty - Receive and process it! + + i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Read the character from the USART + c = R_UDR; + + if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + } + sw_barrier(); + } + + R_UDR = XOFF_CHAR; + + // Clear the TXC bit -- "can be cleared by writing a one to its bit + // location". This makes sure flush() won't return until the bytes + // actually got written + B_TXC = 1; + + // At this point there could be a race condition between the write() function + // and this sending of the XOFF char. This interrupt could happen between the + // wait to be empty TX buffer loop and the actual write of the character. Since + // the TX buffer is full because it's sending the XOFF char, the only way to be + // sure the write() function will succeed is to wait for the XOFF char to be + // completely sent. Since an extra character could be received during the wait + // it must also be handled! + while (!B_UDRE) { + + if (B_RXC) { + // A char arrived while waiting for the TX buffer to be empty - Receive and process it! + + i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Read the character from the USART + c = R_UDR; + + if (Cfg::EMERGENCYPARSER) + emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + } + sw_barrier(); + } + + // At this point everything is ready. The write() function won't + // have any issues writing to the UART TX register if it needs to! + } + } + } + + // Store the new head value - The main loop will retry until the value is stable + rx_buffer.head = h; +} + +// (called with TX irqs disabled) +template +FORCE_INLINE void MarlinSerial::_tx_udr_empty_irq() { + if (Cfg::TX_SIZE > 0) { + // Read positions + uint8_t t = tx_buffer.tail; + const uint8_t h = tx_buffer.head; + + if (Cfg::XONOFF) { + // If an XON char is pending to be sent, do it now + if (xon_xoff_state == XON_CHAR) { + + // Send the character + R_UDR = XON_CHAR; + + // clear the TXC bit -- "can be cleared by writing a one to its bit + // location". This makes sure flush() won't return until the bytes + // actually got written + B_TXC = 1; + + // Remember we sent it. + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + + // If nothing else to transmit, just disable TX interrupts. + if (h == t) B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed) + + return; + } + } + + // If nothing to transmit, just disable TX interrupts. This could + // happen as the result of the non atomicity of the disabling of RX + // interrupts that could end reenabling TX interrupts as a side effect. + if (h == t) { + B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed) + return; + } + + // There is something to TX, Send the next byte + const uint8_t c = tx_buffer.buffer[t]; + t = (t + 1) & (Cfg::TX_SIZE - 1); + R_UDR = c; + tx_buffer.tail = t; + + // Clear the TXC bit (by writing a one to its bit location). + // Ensures flush() won't return until the bytes are actually written/ + B_TXC = 1; + + // Disable interrupts if there is nothing to transmit following this byte + if (h == t) B_UDRIE = 0; // (Non-atomic, could be reenabled by the main program, but eventually this will succeed) + } +} + +// Public Methods +template +void MarlinSerial::begin(const long baud) { + uint16_t baud_setting; + bool useU2X = true; + + #if F_CPU == 16000000UL && SERIAL_PORT == 0 + // Hard-coded exception for compatibility with the bootloader shipped + // with the Duemilanove and previous boards, and the firmware on the + // 8U2 on the Uno and Mega 2560. + if (baud == 57600) useU2X = false; + #endif + + R_UCSRA = 0; + if (useU2X) { + B_U2X = 1; + baud_setting = (F_CPU / 4 / baud - 1) / 2; + } + else + baud_setting = (F_CPU / 8 / baud - 1) / 2; + + // assign the baud_setting, a.k.a. ubbr (USART Baud Rate Register) + R_UBRRH = baud_setting >> 8; + R_UBRRL = baud_setting; + + B_RXEN = 1; + B_TXEN = 1; + B_RXCIE = 1; + if (Cfg::TX_SIZE > 0) B_UDRIE = 0; + _written = false; +} + +template +void MarlinSerial::end() { + B_RXEN = 0; + B_TXEN = 0; + B_RXCIE = 0; + B_UDRIE = 0; +} + +template +int MarlinSerial::peek() { + const ring_buffer_pos_t h = atomic_read_rx_head(), t = rx_buffer.tail; + return h == t ? -1 : rx_buffer.buffer[t]; +} + +template +int MarlinSerial::read() { + const ring_buffer_pos_t h = atomic_read_rx_head(); + + // Read the tail. Main thread owns it, so it is safe to directly read it + ring_buffer_pos_t t = rx_buffer.tail; + + // If nothing to read, return now + if (h == t) return -1; + + // Get the next char + const int v = rx_buffer.buffer[t]; + t = (ring_buffer_pos_t)(t + 1) & (Cfg::RX_SIZE - 1); + + // Advance tail - Making sure the RX ISR will always get an stable value, even + // if it interrupts the writing of the value of that variable in the middle. + atomic_set_rx_tail(t); + + if (Cfg::XONOFF) { + // If the XOFF char was sent, or about to be sent... + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) { + // Get count of bytes in the RX buffer + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + if (rx_count < (Cfg::RX_SIZE) / 10) { + if (Cfg::TX_SIZE > 0) { + // Signal we want an XON character to be sent. + xon_xoff_state = XON_CHAR; + // Enable TX ISR. Non atomic, but it will eventually enable them + B_UDRIE = 1; + } + else { + // If not using TX interrupts, we must send the XON char now + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + while (!B_UDRE) sw_barrier(); + R_UDR = XON_CHAR; + } + } + } + } + + return v; +} + +template +typename MarlinSerial::ring_buffer_pos_t MarlinSerial::available() { + const ring_buffer_pos_t h = atomic_read_rx_head(), t = rx_buffer.tail; + return (ring_buffer_pos_t)(Cfg::RX_SIZE + h - t) & (Cfg::RX_SIZE - 1); +} + +template +void MarlinSerial::flush() { + + // Set the tail to the head: + // - Read the RX head index in a safe way. (See atomic_read_rx_head.) + // - Set the tail, making sure the RX ISR will always get a stable value, even + // if it interrupts the writing of the value of that variable in the middle. + atomic_set_rx_tail(atomic_read_rx_head()); + + if (Cfg::XONOFF) { + // If the XOFF char was sent, or about to be sent... + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) { + if (Cfg::TX_SIZE > 0) { + // Signal we want an XON character to be sent. + xon_xoff_state = XON_CHAR; + // Enable TX ISR. Non atomic, but it will eventually enable it. + B_UDRIE = 1; + } + else { + // If not using TX interrupts, we must send the XON char now + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + while (!B_UDRE) sw_barrier(); + R_UDR = XON_CHAR; + } + } + } +} + +template +void MarlinSerial::write(const uint8_t c) { + if (Cfg::TX_SIZE == 0) { + + _written = true; + while (!B_UDRE) sw_barrier(); + R_UDR = c; + + } + else { + + _written = true; + + // If the TX interrupts are disabled and the data register + // is empty, just write the byte to the data register and + // be done. This shortcut helps significantly improve the + // effective datarate at high (>500kbit/s) bitrates, where + // interrupt overhead becomes a slowdown. + // Yes, there is a race condition between the sending of the + // XOFF char at the RX ISR, but it is properly handled there + if (!B_UDRIE && B_UDRE) { + R_UDR = c; + + // clear the TXC bit -- "can be cleared by writing a one to its bit + // location". This makes sure flush() won't return until the bytes + // actually got written + B_TXC = 1; + return; + } + + const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1); + + // If global interrupts are disabled (as the result of being called from an ISR)... + if (!hal.isr_state()) { + + // Make room by polling if it is possible to transmit, and do so! + while (i == tx_buffer.tail) { + + // If we can transmit another byte, do it. + if (B_UDRE) _tx_udr_empty_irq(); + + // Make sure compiler rereads tx_buffer.tail + sw_barrier(); + } + } + else { + // Interrupts are enabled, just wait until there is space + while (i == tx_buffer.tail) sw_barrier(); + } + + // Store new char. head is always safe to move + tx_buffer.buffer[tx_buffer.head] = c; + tx_buffer.head = i; + + // Enable TX ISR - Non atomic, but it will eventually enable TX ISR + B_UDRIE = 1; + } +} + +template +void MarlinSerial::flushTX() { + + if (Cfg::TX_SIZE == 0) { + // No bytes written, no need to flush. This special case is needed since there's + // no way to force the TXC (transmit complete) bit to 1 during initialization. + if (!_written) return; + + // Wait until everything was transmitted + while (!B_TXC) sw_barrier(); + + // At this point nothing is queued anymore (DRIE is disabled) and + // the hardware finished transmission (TXC is set). + + } + else { + + // No bytes written, no need to flush. This special case is needed since there's + // no way to force the TXC (transmit complete) bit to 1 during initialization. + if (!_written) return; + + // If global interrupts are disabled (as the result of being called from an ISR)... + if (!hal.isr_state()) { + + // Wait until everything was transmitted - We must do polling, as interrupts are disabled + while (tx_buffer.head != tx_buffer.tail || !B_TXC) { + + // If there is more space, send an extra character + if (B_UDRE) _tx_udr_empty_irq(); + + sw_barrier(); + } + + } + else { + // Wait until everything was transmitted + while (tx_buffer.head != tx_buffer.tail || !B_TXC) sw_barrier(); + } + + // At this point nothing is queued anymore (DRIE is disabled) and + // the hardware finished transmission (TXC is set). + } +} + +// Hookup ISR handlers +ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _RX_vect)) { + MarlinSerial>::store_rxd_char(); +} + +ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); +} + +// Because of the template definition above, it's required to instantiate the template to have all methods generated +template class MarlinSerial< MarlinSerialCfg >; +MSerialT1 customizedSerial1(MSerialT1::HasEmergencyParser); + +#ifdef SERIAL_PORT_2 + + // Hookup ISR handlers + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_2, _RX_vect)) { + MarlinSerial>::store_rxd_char(); + } + + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_2, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); + } + + template class MarlinSerial< MarlinSerialCfg >; + MSerialT2 customizedSerial2(MSerialT2::HasEmergencyParser); + +#endif // SERIAL_PORT_2 + +#ifdef SERIAL_PORT_3 + + // Hookup ISR handlers + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_3, _RX_vect)) { + MarlinSerial>::store_rxd_char(); + } + + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_3, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); + } + + template class MarlinSerial< MarlinSerialCfg >; + MSerialT3 customizedSerial3(MSerialT3::HasEmergencyParser); + +#endif // SERIAL_PORT_3 + +#ifdef MMU2_SERIAL_PORT + + ISR(SERIAL_REGNAME(USART, MMU2_SERIAL_PORT, _RX_vect)) { + MarlinSerial>::store_rxd_char(); + } + + ISR(SERIAL_REGNAME(USART, MMU2_SERIAL_PORT, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); + } + + template class MarlinSerial< MMU2SerialCfg >; + MSerialMMU2 mmuSerial(MSerialMMU2::HasEmergencyParser); + +#endif // MMU2_SERIAL_PORT + +#ifdef LCD_SERIAL_PORT + + ISR(SERIAL_REGNAME(USART, LCD_SERIAL_PORT, _RX_vect)) { + MarlinSerial>::store_rxd_char(); + } + + ISR(SERIAL_REGNAME(USART, LCD_SERIAL_PORT, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); + } + + template class MarlinSerial< LCDSerialCfg >; + MSerialLCD lcdSerial(MSerialLCD::HasEmergencyParser); + + #if HAS_DGUS_LCD + template + typename MarlinSerial::ring_buffer_pos_t MarlinSerial::get_tx_buffer_free() { + const ring_buffer_pos_t t = tx_buffer.tail, // next byte to send. + h = tx_buffer.head; // next pos for queue. + int ret = t - h - 1; + if (ret < 0) ret += Cfg::TX_SIZE + 1; + return ret; + } + #endif + +#endif // LCD_SERIAL_PORT + +#endif // !USBCON && (UBRRH || UBRR0H || UBRR1H || UBRR2H || UBRR3H) + +// For AT90USB targets use the UART for BT interfacing +#if defined(USBCON) && ENABLED(BLUETOOTH) + MSerialBT bluetoothSerial(false); +#endif + +#endif // __AVR__ diff --git a/src/HAL/AVR/MarlinSerial.h b/src/HAL/AVR/MarlinSerial.h new file mode 100644 index 0000000..7eb7600 --- /dev/null +++ b/src/HAL/AVR/MarlinSerial.h @@ -0,0 +1,297 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * MarlinSerial.h - Hardware serial library for Wiring + * Copyright (c) 2006 Nicholas Zambetti. All right reserved. + * + * Modified 28 September 2010 by Mark Sproul + * Modified 14 February 2016 by Andreas Hardtung (added tx buffer) + * Modified 01 October 2017 by Eduardo José Tagle (added XON/XOFF) + * Templatized 01 October 2018 by Eduardo José Tagle to allow multiple instances + */ + +#include + +#include "../../inc/MarlinConfigPre.h" +#include "../../core/serial_hook.h" + +#ifndef SERIAL_PORT + #define SERIAL_PORT 0 +#endif + +#ifndef USBCON + + // The presence of the UBRRH register is used to detect a UART. + #define UART_PRESENT(port) ((port == 0 && (defined(UBRRH) || defined(UBRR0H))) || \ + (port == 1 && defined(UBRR1H)) || (port == 2 && defined(UBRR2H)) || \ + (port == 3 && defined(UBRR3H))) + + // These are macros to build serial port register names for the selected SERIAL_PORT (C preprocessor + // requires two levels of indirection to expand macro values properly) + #define SERIAL_REGNAME(registerbase,number,suffix) _SERIAL_REGNAME(registerbase,number,suffix) + #if SERIAL_PORT == 0 && (!defined(UBRR0H) || !defined(UDR0)) // use un-numbered registers if necessary + #define _SERIAL_REGNAME(registerbase,number,suffix) registerbase##suffix + #else + #define _SERIAL_REGNAME(registerbase,number,suffix) registerbase##number##suffix + #endif + + // Registers used by MarlinSerial class (expanded depending on selected serial port) + + // Templated 8bit register (generic) + #define UART_REGISTER_DECL_BASE(registerbase, suffix) \ + template struct R_##registerbase##x##suffix {} + + // Templated 8bit register (specialization for each port) + #define UART_REGISTER_DECL(port, registerbase, suffix) \ + template<> struct R_##registerbase##x##suffix { \ + constexpr R_##registerbase##x##suffix(int) {} \ + FORCE_INLINE void operator=(uint8_t newVal) const { SERIAL_REGNAME(registerbase,port,suffix) = newVal; } \ + FORCE_INLINE operator uint8_t() const { return SERIAL_REGNAME(registerbase,port,suffix); } \ + } + + // Templated 1bit register (generic) + #define UART_BIT_DECL_BASE(registerbase, suffix, bit) \ + templatestruct B_##bit##x {} + + // Templated 1bit register (specialization for each port) + #define UART_BIT_DECL(port, registerbase, suffix, bit) \ + template<> struct B_##bit##x { \ + constexpr B_##bit##x(int) {} \ + FORCE_INLINE void operator=(int newVal) const { \ + if (newVal) \ + SBI(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); \ + else \ + CBI(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); \ + } \ + FORCE_INLINE operator bool() const { return TEST(SERIAL_REGNAME(registerbase,port,suffix),SERIAL_REGNAME(bit,port,)); } \ + } + + #define UART_DECL_BASE() \ + UART_REGISTER_DECL_BASE(UCSR,A);\ + UART_REGISTER_DECL_BASE(UDR,);\ + UART_REGISTER_DECL_BASE(UBRR,H);\ + UART_REGISTER_DECL_BASE(UBRR,L);\ + UART_BIT_DECL_BASE(UCSR,B,RXEN);\ + UART_BIT_DECL_BASE(UCSR,B,TXEN);\ + UART_BIT_DECL_BASE(UCSR,A,TXC);\ + UART_BIT_DECL_BASE(UCSR,B,RXCIE);\ + UART_BIT_DECL_BASE(UCSR,A,UDRE);\ + UART_BIT_DECL_BASE(UCSR,A,FE);\ + UART_BIT_DECL_BASE(UCSR,A,DOR);\ + UART_BIT_DECL_BASE(UCSR,B,UDRIE);\ + UART_BIT_DECL_BASE(UCSR,A,RXC);\ + UART_BIT_DECL_BASE(UCSR,A,U2X) + + #define UART_DECL(port) \ + UART_REGISTER_DECL(port,UCSR,A);\ + UART_REGISTER_DECL(port,UDR,);\ + UART_REGISTER_DECL(port,UBRR,H);\ + UART_REGISTER_DECL(port,UBRR,L);\ + UART_BIT_DECL(port,UCSR,B,RXEN);\ + UART_BIT_DECL(port,UCSR,B,TXEN);\ + UART_BIT_DECL(port,UCSR,A,TXC);\ + UART_BIT_DECL(port,UCSR,B,RXCIE);\ + UART_BIT_DECL(port,UCSR,A,UDRE);\ + UART_BIT_DECL(port,UCSR,A,FE);\ + UART_BIT_DECL(port,UCSR,A,DOR);\ + UART_BIT_DECL(port,UCSR,B,UDRIE);\ + UART_BIT_DECL(port,UCSR,A,RXC);\ + UART_BIT_DECL(port,UCSR,A,U2X) + + // Declare empty templates + UART_DECL_BASE(); + + // And all the specializations for each possible serial port + #if UART_PRESENT(0) + UART_DECL(0); + #endif + #if UART_PRESENT(1) + UART_DECL(1); + #endif + #if UART_PRESENT(2) + UART_DECL(2); + #endif + #if UART_PRESENT(3) + UART_DECL(3); + #endif + + #define BYTE 0 + + // Templated type selector + template struct TypeSelector { typedef T type;} ; + template struct TypeSelector { typedef F type; }; + + template + class MarlinSerial { + protected: + // Registers + static constexpr R_UCSRxA R_UCSRA = 0; + static constexpr R_UDRx R_UDR = 0; + static constexpr R_UBRRxH R_UBRRH = 0; + static constexpr R_UBRRxL R_UBRRL = 0; + + // Bits + static constexpr B_RXENx B_RXEN = 0; + static constexpr B_TXENx B_TXEN = 0; + static constexpr B_TXCx B_TXC = 0; + static constexpr B_RXCIEx B_RXCIE = 0; + static constexpr B_UDREx B_UDRE = 0; + static constexpr B_FEx B_FE = 0; + static constexpr B_DORx B_DOR = 0; + static constexpr B_UDRIEx B_UDRIE = 0; + static constexpr B_RXCx B_RXC = 0; + static constexpr B_U2Xx B_U2X = 0; + + // Base size of type on buffer size + typedef typename TypeSelector<(Cfg::RX_SIZE>256), uint16_t, uint8_t>::type ring_buffer_pos_t; + + struct ring_buffer_r { + volatile ring_buffer_pos_t head, tail; + unsigned char buffer[Cfg::RX_SIZE]; + }; + + struct ring_buffer_t { + volatile uint8_t head, tail; + unsigned char buffer[Cfg::TX_SIZE]; + }; + + static ring_buffer_r rx_buffer; + static ring_buffer_t tx_buffer; + static bool _written; + + static constexpr uint8_t XON_XOFF_CHAR_SENT = 0x80, // XON / XOFF Character was sent + XON_XOFF_CHAR_MASK = 0x1F; // XON / XOFF character to send + + // XON / XOFF character definitions + static constexpr uint8_t XON_CHAR = 17, XOFF_CHAR = 19; + static uint8_t xon_xoff_state, + rx_dropped_bytes, + rx_buffer_overruns, + rx_framing_errors; + static ring_buffer_pos_t rx_max_enqueued; + + FORCE_INLINE static ring_buffer_pos_t atomic_read_rx_head(); + + static volatile bool rx_tail_value_not_stable; + static volatile uint16_t rx_tail_value_backup; + + FORCE_INLINE static void atomic_set_rx_tail(ring_buffer_pos_t value); + FORCE_INLINE static ring_buffer_pos_t atomic_read_rx_tail(); + + public: + FORCE_INLINE static void store_rxd_char(); + FORCE_INLINE static void _tx_udr_empty_irq(); + + public: + static void begin(const long); + static void end(); + static int peek(); + static int read(); + static void flush(); + static ring_buffer_pos_t available(); + static void write(const uint8_t c); + static void flushTX(); + #if HAS_DGUS_LCD + static ring_buffer_pos_t get_tx_buffer_free(); + #endif + + enum { HasEmergencyParser = Cfg::EMERGENCYPARSER }; + static bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } + + FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; } + FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; } + FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; } + FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; } + }; + + template + struct MarlinSerialCfg { + static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = RX_BUFFER_SIZE; + static constexpr unsigned int TX_SIZE = TX_BUFFER_SIZE; + static constexpr bool XONOFF = ENABLED(SERIAL_XON_XOFF); + static constexpr bool EMERGENCYPARSER = ENABLED(EMERGENCY_PARSER); + static constexpr bool DROPPED_RX = ENABLED(SERIAL_STATS_DROPPED_RX); + static constexpr bool RX_OVERRUNS = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS); + static constexpr bool RX_FRAMING_ERRORS = ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS); + static constexpr bool MAX_RX_QUEUED = ENABLED(SERIAL_STATS_MAX_RX_QUEUED); + }; + + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT1; + extern MSerialT1 customizedSerial1; + + #ifdef SERIAL_PORT_2 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT2; + extern MSerialT2 customizedSerial2; + #endif + + #ifdef SERIAL_PORT_3 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT3; + extern MSerialT3 customizedSerial3; + #endif + +#endif // !USBCON + +#ifdef MMU2_SERIAL_PORT + template + struct MMU2SerialCfg { + static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = 32; + static constexpr unsigned int TX_SIZE = 32; + static constexpr bool XONOFF = false; + static constexpr bool EMERGENCYPARSER = false; + static constexpr bool DROPPED_RX = false; + static constexpr bool RX_FRAMING_ERRORS = false; + static constexpr bool MAX_RX_QUEUED = false; + static constexpr bool RX_OVERRUNS = false; + }; + + typedef Serial1Class< MarlinSerial< MMU2SerialCfg > > MSerialMMU2; + extern MSerialMMU2 mmuSerial; +#endif + +#ifdef LCD_SERIAL_PORT + + template + struct LCDSerialCfg { + static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = TERN(HAS_DGUS_LCD, DGUS_RX_BUFFER_SIZE, 64); + static constexpr unsigned int TX_SIZE = TERN(HAS_DGUS_LCD, DGUS_TX_BUFFER_SIZE, 128); + static constexpr bool XONOFF = false; + static constexpr bool EMERGENCYPARSER = ENABLED(EMERGENCY_PARSER); + static constexpr bool DROPPED_RX = false; + static constexpr bool RX_FRAMING_ERRORS = false; + static constexpr bool MAX_RX_QUEUED = false; + static constexpr bool RX_OVERRUNS = BOTH(HAS_DGUS_LCD, SERIAL_STATS_RX_BUFFER_OVERRUNS); + }; + + typedef Serial1Class< MarlinSerial< LCDSerialCfg > > MSerialLCD; + extern MSerialLCD lcdSerial; +#endif + +// Use the UART for Bluetooth in AT90USB configurations +#if defined(USBCON) && ENABLED(BLUETOOTH) + typedef Serial1Class MSerialBT; + extern MSerialBT bluetoothSerial; +#endif diff --git a/src/HAL/AVR/Servo.cpp b/src/HAL/AVR/Servo.cpp new file mode 100644 index 0000000..0a1ef53 --- /dev/null +++ b/src/HAL/AVR/Servo.cpp @@ -0,0 +1,226 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + */ + +/** + * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. + * The servos are pulsed in the background using the value most recently written using the write() method + * + * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. + * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. + * + * The methods are: + * + * Servo - Class for manipulating servo motors connected to Arduino pins. + * + * attach(pin) - Attach a servo motor to an i/o pin. + * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds + * Default min is 544, max is 2400 + * + * write() - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.) + * writeMicroseconds() - Set the servo pulse width in microseconds. + * move(pin, angle) - Sequence of attach(pin), write(angle), safe_delay(servo_delay[servoIndex]). + * With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after servo_delay[servoIndex]. + * read() - Get the last-written servo pulse width as an angle between 0 and 180. + * readMicroseconds() - Get the last-written servo pulse width in microseconds. + * attached() - Return true if a servo is attached. + * detach() - Stop an attached servo from pulsing its i/o pin. + */ + +#ifdef __AVR__ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include + +#include "../shared/servo.h" +#include "../shared/servo_private.h" + +static volatile int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval) + + +/************ static functions common to all instances ***********************/ + +static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { + int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first + if (cho < 0) // Channel -1 indicates the refresh interval completed... + *TCNTn = 0; // ...so reset the timer + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW + + Channel[timer] = ++cho; // Handle the next channel (or 0) + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { + *OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration + if (SERVO(timer, cho).Pin.isActive) // activated? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH + } + else { + // finished all channels so wait for the refresh period to expire before starting over + const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed + *OCRnA = max(cval, ival); + + Channel[timer] = -1; // reset the timer counter to 0 on the next call + } +} + +#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform + + // Interrupt handlers for Arduino + #ifdef _useTimer1 + SIGNAL(TIMER1_COMPA_vect) { handle_interrupts(_timer1, &TCNT1, &OCR1A); } + #endif + + #ifdef _useTimer3 + SIGNAL(TIMER3_COMPA_vect) { handle_interrupts(_timer3, &TCNT3, &OCR3A); } + #endif + + #ifdef _useTimer4 + SIGNAL(TIMER4_COMPA_vect) { handle_interrupts(_timer4, &TCNT4, &OCR4A); } + #endif + + #ifdef _useTimer5 + SIGNAL(TIMER5_COMPA_vect) { handle_interrupts(_timer5, &TCNT5, &OCR5A); } + #endif + +#else // WIRING + + // Interrupt handlers for Wiring + #ifdef _useTimer1 + void Timer1Service() { handle_interrupts(_timer1, &TCNT1, &OCR1A); } + #endif + #ifdef _useTimer3 + void Timer3Service() { handle_interrupts(_timer3, &TCNT3, &OCR3A); } + #endif + +#endif // WIRING + +/****************** end of static functions ******************************/ + +void initISR(const timer16_Sequence_t timer_index) { + switch (timer_index) { + default: break; + + #ifdef _useTimer1 + case _timer1: + TCCR1A = 0; // normal counting mode + TCCR1B = _BV(CS11); // set prescaler of 8 + TCNT1 = 0; // clear the timer count + #if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__) + SBI(TIFR, OCF1A); // clear any pending interrupts; + SBI(TIMSK, OCIE1A); // enable the output compare interrupt + #else + // here if not ATmega8 or ATmega128 + SBI(TIFR1, OCF1A); // clear any pending interrupts; + SBI(TIMSK1, OCIE1A); // enable the output compare interrupt + #endif + #ifdef WIRING + timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service); + #endif + break; + #endif + + #ifdef _useTimer3 + case _timer3: + TCCR3A = 0; // normal counting mode + TCCR3B = _BV(CS31); // set prescaler of 8 + TCNT3 = 0; // clear the timer count + #ifdef __AVR_ATmega128__ + SBI(TIFR, OCF3A); // clear any pending interrupts; + SBI(ETIMSK, OCIE3A); // enable the output compare interrupt + #else + SBI(TIFR3, OCF3A); // clear any pending interrupts; + SBI(TIMSK3, OCIE3A); // enable the output compare interrupt + #endif + #ifdef WIRING + timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only + #endif + break; + #endif + + #ifdef _useTimer4 + case _timer4: + TCCR4A = 0; // normal counting mode + TCCR4B = _BV(CS41); // set prescaler of 8 + TCNT4 = 0; // clear the timer count + TIFR4 = _BV(OCF4A); // clear any pending interrupts; + TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt + break; + #endif + + #ifdef _useTimer5 + case _timer5: + TCCR5A = 0; // normal counting mode + TCCR5B = _BV(CS51); // set prescaler of 8 + TCNT5 = 0; // clear the timer count + TIFR5 = _BV(OCF5A); // clear any pending interrupts; + TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt + break; + #endif + } +} + +void finISR(const timer16_Sequence_t timer_index) { + // Disable use of the given timer + #ifdef WIRING + switch (timer_index) { + default: break; + + case _timer1: + CBI( + #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + TIMSK1 + #else + TIMSK + #endif + , OCIE1A // disable timer 1 output compare interrupt + ); + timerDetach(TIMER1OUTCOMPAREA_INT); + break; + + case _timer3: + CBI( + #if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + TIMSK3 + #else + ETIMSK + #endif + , OCIE3A // disable the timer3 output compare A interrupt + ); + timerDetach(TIMER3OUTCOMPAREA_INT); + break; + } + #else // !WIRING + // For arduino - in future: call here to a currently undefined function to reset the timer + UNUSED(timer_index); + #endif +} + +#endif // HAS_SERVOS + +#endif // __AVR__ diff --git a/src/HAL/AVR/ServoTimers.h b/src/HAL/AVR/ServoTimers.h new file mode 100644 index 0000000..436b281 --- /dev/null +++ b/src/HAL/AVR/ServoTimers.h @@ -0,0 +1,93 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * ServoTimers.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * Defines for 16 bit timers used with Servo library + * + * If _useTimerX is defined then TimerX is a 16 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many 16 bit timers are available. + */ + +/** + * AVR Only definitions + * -------------------- + */ + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays +#define SERVO_TIMER_PRESCALER 8 // timer prescaler + +// Say which 16 bit timers can be used and in what order +#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + //#define _useTimer1 + #define _useTimer4 + #if NUM_SERVOS > SERVOS_PER_TIMER + #define _useTimer3 + #if !HAS_MOTOR_CURRENT_PWM && SERVOS > 2 * SERVOS_PER_TIMER + #define _useTimer5 // Timer 5 is used for motor current PWM and can't be used for servos. + #endif + #endif +#elif defined(__AVR_ATmega32U4__) + #define _useTimer3 +#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) + #define _useTimer3 +#elif defined(__AVR_ATmega128__) || defined(__AVR_ATmega1281__) || defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__) + #define _useTimer3 +#else + // everything else +#endif + +typedef enum { + #ifdef _useTimer1 + _timer1, + #endif + #ifdef _useTimer3 + _timer3, + #endif + #ifdef _useTimer4 + _timer4, + #endif + #ifdef _useTimer5 + _timer5, + #endif + _Nbr_16timers +} timer16_Sequence_t; diff --git a/src/HAL/AVR/eeprom.cpp b/src/HAL/AVR/eeprom.cpp new file mode 100644 index 0000000..8d084de --- /dev/null +++ b/src/HAL/AVR/eeprom.cpp @@ -0,0 +1,74 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __AVR__ + +#include "../../inc/MarlinConfig.h" + +#if EITHER(EEPROM_SETTINGS, SD_FIRMWARE_UPDATE) + +/** + * PersistentStore for Arduino-style EEPROM interface + * with implementations supplied by the framework. + */ + +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } +bool PersistentStore::access_start() { return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; // always assume success for AVR's +} + +#endif // EEPROM_SETTINGS || SD_FIRMWARE_UPDATE +#endif // __AVR__ diff --git a/src/HAL/AVR/endstop_interrupts.h b/src/HAL/AVR/endstop_interrupts.h new file mode 100644 index 0000000..0ce8574 --- /dev/null +++ b/src/HAL/AVR/endstop_interrupts.h @@ -0,0 +1,306 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate either an + * 'external interrupt' or a 'pin change interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +#include + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +/** + * Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h) + * + * These macros for the Arduino MEGA do not include the two connected pins on Port J (D14, D15). + * So we extend them here because these are the normal pins for Y_MIN and Y_MAX on RAMPS. + * There are more PCI-enabled processor pins on Port J, but they are not connected to Arduino MEGA. + */ +#if defined(ARDUINO_AVR_MEGA2560) || defined(ARDUINO_AVR_MEGA) + + #define digitalPinHasPCICR(p) (WITHIN(p, 10, 15) || WITHIN(p, 50, 53) || WITHIN(p, 62, 69)) + + #undef digitalPinToPCICR + #define digitalPinToPCICR(p) (digitalPinHasPCICR(p) ? (&PCICR) : nullptr) + + #undef digitalPinToPCICRbit + #define digitalPinToPCICRbit(p) (WITHIN(p, 10, 13) || WITHIN(p, 50, 53) ? 0 : \ + WITHIN(p, 14, 15) ? 1 : \ + WITHIN(p, 62, 69) ? 2 : \ + 0) + + #undef digitalPinToPCMSK + #define digitalPinToPCMSK(p) (WITHIN(p, 10, 13) || WITHIN(p, 50, 53) ? (&PCMSK0) : \ + WITHIN(p, 14, 15) ? (&PCMSK1) : \ + WITHIN(p, 62, 69) ? (&PCMSK2) : \ + nullptr) + + #undef digitalPinToPCMSKbit + #define digitalPinToPCMSKbit(p) (WITHIN(p, 10, 13) ? ((p) - 6) : \ + (p) == 14 || (p) == 51 ? 2 : \ + (p) == 15 || (p) == 52 ? 1 : \ + (p) == 50 ? 3 : \ + (p) == 53 ? 0 : \ + WITHIN(p, 62, 69) ? ((p) - 62) : \ + 0) + +#elif defined(__AVR_ATmega164A__) || defined(__AVR_ATmega164P__) || defined(__AVR_ATmega324A__) || \ + defined(__AVR_ATmega324P__) || defined(__AVR_ATmega324PA__) || defined(__AVR_ATmega324PB__) || \ + defined(__AVR_ATmega644A__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega1284__) || \ + defined(__AVR_ATmega1284P__) + + #define digitalPinHasPCICR(p) WITHIN(p, 0, NUM_DIGITAL_PINS) + +#else + + #error "Unsupported AVR variant!" + +#endif + + +// Install Pin change interrupt for a pin. Can be called multiple times. +void pciSetup(const int8_t pin) { + if (digitalPinHasPCICR(pin)) { + SBI(*digitalPinToPCMSK(pin), digitalPinToPCMSKbit(pin)); // enable pin + SBI(PCIFR, digitalPinToPCICRbit(pin)); // clear any outstanding interrupt + SBI(PCICR, digitalPinToPCICRbit(pin)); // enable interrupt for the group + } +} + +// Handlers for pin change interrupts +#ifdef PCINT0_vect + ISR(PCINT0_vect) { endstop_ISR(); } +#endif + +#ifdef PCINT1_vect + ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect)); +#endif + +#ifdef PCINT2_vect + ISR(PCINT2_vect, ISR_ALIASOF(PCINT0_vect)); +#endif + +#ifdef PCINT3_vect + ISR(PCINT3_vect, ISR_ALIASOF(PCINT0_vect)); +#endif + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + #if HAS_X_MAX + #if (digitalPinToInterrupt(X_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(X_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(X_MAX_PIN), "X_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(X_MAX_PIN); + #endif + #endif + #if HAS_X_MIN + #if (digitalPinToInterrupt(X_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(X_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(X_MIN_PIN), "X_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(X_MIN_PIN); + #endif + #endif + #if HAS_Y_MAX + #if (digitalPinToInterrupt(Y_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Y_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Y_MAX_PIN), "Y_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Y_MAX_PIN); + #endif + #endif + #if HAS_Y_MIN + #if (digitalPinToInterrupt(Y_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Y_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Y_MIN_PIN), "Y_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Y_MIN_PIN); + #endif + #endif + #if HAS_Z_MAX + #if (digitalPinToInterrupt(Z_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Z_MAX_PIN), "Z_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z_MAX_PIN); + #endif + #endif + #if HAS_Z_MIN + #if (digitalPinToInterrupt(Z_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Z_MIN_PIN), "Z_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z_MIN_PIN); + #endif + #endif + #if HAS_I_MAX + #if (digitalPinToInterrupt(I_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(I_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(I_MAX_PIN), "I_MAX_PIN is not interrupt-capable"); + pciSetup(I_MAX_PIN); + #endif + #elif HAS_I_MIN + #if (digitalPinToInterrupt(I_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(I_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(I_MIN_PIN), "I_MIN_PIN is not interrupt-capable"); + pciSetup(I_MIN_PIN); + #endif + #endif + #if HAS_J_MAX + #if (digitalPinToInterrupt(J_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(J_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(J_MAX_PIN), "J_MAX_PIN is not interrupt-capable"); + pciSetup(J_MAX_PIN); + #endif + #elif HAS_J_MIN + #if (digitalPinToInterrupt(J_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(J_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(J_MIN_PIN), "J_MIN_PIN is not interrupt-capable"); + pciSetup(J_MIN_PIN); + #endif + #endif + #if HAS_K_MAX + #if (digitalPinToInterrupt(K_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(K_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(K_MAX_PIN), "K_MAX_PIN is not interrupt-capable"); + pciSetup(K_MAX_PIN); + #endif + #elif HAS_K_MIN + #if (digitalPinToInterrupt(K_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(K_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(K_MIN_PIN), "K_MIN_PIN is not interrupt-capable"); + pciSetup(K_MIN_PIN); + #endif + #endif + #if HAS_X2_MAX + #if (digitalPinToInterrupt(X2_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(X2_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(X2_MAX_PIN), "X2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(X2_MAX_PIN); + #endif + #endif + #if HAS_X2_MIN + #if (digitalPinToInterrupt(X2_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(X2_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(X2_MIN_PIN), "X2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(X2_MIN_PIN); + #endif + #endif + #if HAS_Y2_MAX + #if (digitalPinToInterrupt(Y2_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Y2_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Y2_MAX_PIN), "Y2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Y2_MAX_PIN); + #endif + #endif + #if HAS_Y2_MIN + #if (digitalPinToInterrupt(Y2_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Y2_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Y2_MIN_PIN), "Y2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Y2_MIN_PIN); + #endif + #endif + #if HAS_Z2_MAX + #if (digitalPinToInterrupt(Z2_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z2_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Z2_MAX_PIN), "Z2_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z2_MAX_PIN); + #endif + #endif + #if HAS_Z2_MIN + #if (digitalPinToInterrupt(Z2_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z2_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Z2_MIN_PIN), "Z2_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z2_MIN_PIN); + #endif + #endif + #if HAS_Z3_MAX + #if (digitalPinToInterrupt(Z3_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z3_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Z3_MAX_PIN), "Z3_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z3_MAX_PIN); + #endif + #endif + #if HAS_Z3_MIN + #if (digitalPinToInterrupt(Z3_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z3_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Z3_MIN_PIN), "Z3_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z3_MIN_PIN); + #endif + #endif + #if HAS_Z4_MAX + #if (digitalPinToInterrupt(Z4_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z4_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(Z4_MAX_PIN), "Z4_MAX_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z4_MAX_PIN); + #endif + #endif + #if HAS_Z4_MIN + #if (digitalPinToInterrupt(Z4_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z4_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(Z4_MIN_PIN), "Z4_MIN_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z4_MIN_PIN); + #endif + #endif + #if HAS_Z_MIN_PROBE_PIN + #if (digitalPinToInterrupt(Z_MIN_PROBE_PIN) != NOT_AN_INTERRUPT) + _ATTACH(Z_MIN_PROBE_PIN); + #else + static_assert(digitalPinHasPCICR(Z_MIN_PROBE_PIN), "Z_MIN_PROBE_PIN is not interrupt-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue."); + pciSetup(Z_MIN_PROBE_PIN); + #endif + #endif + + // If we arrive here without raising an assertion, each pin has either an EXT-interrupt or a PCI. +} diff --git a/src/HAL/AVR/fast_pwm.cpp b/src/HAL/AVR/fast_pwm.cpp new file mode 100644 index 0000000..0a38417 --- /dev/null +++ b/src/HAL/AVR/fast_pwm.cpp @@ -0,0 +1,222 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __AVR__ + +#include "../../inc/MarlinConfig.h" + +struct Timer { + volatile uint8_t* TCCRnQ[3]; // max 3 TCCR registers per timer + volatile uint16_t* OCRnQ[3]; // max 3 OCR registers per timer + volatile uint16_t* ICRn; // max 1 ICR register per timer + uint8_t n; // the timer number [0->5] + uint8_t q; // the timer output [0->2] (A->C) + bool isPWM; // True if pin is a "hardware timer" + bool isProtected; // True if timer is protected +}; + +// Macros for the Timer structure +#define _SET_WGMnQ(T, V) do{ \ + *(T.TCCRnQ)[0] = (*(T.TCCRnQ)[0] & ~(0x3 << 0)) | (( int(V) & 0x3) << 0); \ + *(T.TCCRnQ)[1] = (*(T.TCCRnQ)[1] & ~(0x3 << 3)) | (((int(V) >> 2) & 0x3) << 3); \ + }while(0) + +// Set TCCR CS bits +#define _SET_CSn(T, V) (*(T.TCCRnQ)[1] = (*(T.TCCRnQ[1]) & ~(0x7 << 0)) | ((int(V) & 0x7) << 0)) + +// Set TCCR COM bits +#define _SET_COMnQ(T, Q, V) (*(T.TCCRnQ)[0] = (*(T.TCCRnQ)[0] & ~(0x3 << (6-2*(Q)))) | (int(V) << (6-2*(Q)))) + +// Set OCRnQ register +#define _SET_OCRnQ(T, Q, V) (*(T.OCRnQ)[Q] = int(V) & 0xFFFF) + +// Set ICRn register (one per timer) +#define _SET_ICRn(T, V) (*(T.ICRn) = int(V) & 0xFFFF) + +/** + * Return a Timer struct describing a pin's timer. + */ +const Timer get_pwm_timer(const pin_t pin) { + + uint8_t q = 0; + + switch (digitalPinToTimer(pin)) { + #ifdef TCCR0A + IF_DISABLED(AVR_AT90USB1286_FAMILY, case TIMER0A:) + #endif + #ifdef TCCR1A + case TIMER1A: case TIMER1B: + #endif + + break; // Protect reserved timers (TIMER0 & TIMER1) + + #ifdef TCCR0A + case TIMER0B: // Protected timer, but allow setting the duty cycle on OCR0B for pin D4 only + return Timer({ { &TCCR0A, nullptr, nullptr }, { (uint16_t*)&OCR0A, (uint16_t*)&OCR0B, nullptr }, nullptr, 0, 1, true, true }); + #endif + + #if HAS_TCCR2 + case TIMER2: + return Timer({ { &TCCR2, nullptr, nullptr }, { (uint16_t*)&OCR2, nullptr, nullptr }, nullptr, 2, 0, true, false }); + #elif ENABLED(USE_OCR2A_AS_TOP) + case TIMER2A: break; // Protect TIMER2A since its OCR is used by TIMER2B + case TIMER2B: + return Timer({ { &TCCR2A, &TCCR2B, nullptr }, { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, nullptr, 2, 1, true, false }); + #elif defined(TCCR2A) + case TIMER2B: ++q; case TIMER2A: + return Timer({ { &TCCR2A, &TCCR2B, nullptr }, { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, nullptr, 2, q, true, false }); + #endif + + #ifdef OCR3C + case TIMER3C: ++q; case TIMER3B: ++q; case TIMER3A: + return Timer({ { &TCCR3A, &TCCR3B, &TCCR3C }, { &OCR3A, &OCR3B, &OCR3C }, &ICR3, 3, q, true, false }); + #elif defined(OCR3B) + case TIMER3B: ++q; case TIMER3A: + return Timer({ { &TCCR3A, &TCCR3B, nullptr }, { &OCR3A, &OCR3B, nullptr }, &ICR3, 3, q, true, false }); + #endif + + #ifdef TCCR4A + case TIMER4C: ++q; case TIMER4B: ++q; case TIMER4A: + return Timer({ { &TCCR4A, &TCCR4B, &TCCR4C }, { &OCR4A, &OCR4B, &OCR4C }, &ICR4, 4, q, true, false }); + #endif + + #ifdef TCCR5A + case TIMER5C: ++q; case TIMER5B: ++q; case TIMER5A: + return Timer({ { &TCCR5A, &TCCR5B, &TCCR5C }, { &OCR5A, &OCR5B, &OCR5C }, &ICR5, 5, q, true, false }); + #endif + } + + return Timer(); +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + const Timer timer = get_pwm_timer(pin); + if (timer.isProtected || !timer.isPWM) return; // Don't proceed if protected timer or not recognized + + const bool is_timer2 = timer.n == 2; + const uint16_t maxtop = is_timer2 ? 0xFF : 0xFFFF; + + uint16_t res = 0xFF; // resolution (TOP value) + uint8_t j = CS_NONE; // prescaler index + uint8_t wgm = WGM_PWM_PC_8; // waveform generation mode + + // Calculating the prescaler and resolution to use to achieve closest frequency + if (f_desired != 0) { + constexpr uint16_t prescaler[] = { 1, 8, (32), 64, (128), 256, 1024 }; // (*) are Timer 2 only + uint16_t f = (F_CPU) / (2 * 1024 * maxtop) + 1; // Start with the lowest non-zero frequency achievable (1 or 31) + + LOOP_L_N(i, COUNT(prescaler)) { // Loop through all prescaler values + const uint16_t p = prescaler[i]; + uint16_t res_fast_temp, res_pc_temp; + if (is_timer2) { + #if ENABLED(USE_OCR2A_AS_TOP) // No resolution calculation for TIMER2 unless enabled USE_OCR2A_AS_TOP + const uint16_t rft = (F_CPU) / (p * f_desired); + res_fast_temp = rft - 1; + res_pc_temp = rft / 2; + #else + res_fast_temp = res_pc_temp = maxtop; + #endif + } + else { + if (p == 32 || p == 128) continue; // Skip TIMER2 specific prescalers when not TIMER2 + const uint16_t rft = (F_CPU) / (p * f_desired); + res_fast_temp = rft - 1; + res_pc_temp = rft / 2; + } + + LIMIT(res_fast_temp, 1U, maxtop); + LIMIT(res_pc_temp, 1U, maxtop); + + // Calculate frequencies of test prescaler and resolution values + const uint32_t f_diff = _MAX(f, f_desired) - _MIN(f, f_desired), + f_fast_temp = (F_CPU) / (p * (1 + res_fast_temp)), + f_fast_diff = _MAX(f_fast_temp, f_desired) - _MIN(f_fast_temp, f_desired), + f_pc_temp = (F_CPU) / (2 * p * res_pc_temp), + f_pc_diff = _MAX(f_pc_temp, f_desired) - _MIN(f_pc_temp, f_desired); + + if (f_fast_diff < f_diff && f_fast_diff <= f_pc_diff) { // FAST values are closest to desired f + // Set the Wave Generation Mode to FAST PWM + wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_FAST_PWM_OCR2A, WGM2_FAST_PWM)) : uint8_t(WGM_FAST_PWM_ICRn); + // Remember this combination + f = f_fast_temp; res = res_fast_temp; j = i + 1; + } + else if (f_pc_diff < f_diff) { // PHASE CORRECT values are closes to desired f + // Set the Wave Generation Mode to PWM PHASE CORRECT + wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_PWM_PC_OCR2A, WGM2_PWM_PC)) : uint8_t(WGM_PWM_PC_ICRn); + f = f_pc_temp; res = res_pc_temp; j = i + 1; + } + } + } + + _SET_WGMnQ(timer, wgm); + _SET_CSn(timer, j); + + if (is_timer2) { + TERN_(USE_OCR2A_AS_TOP, _SET_OCRnQ(timer, 0, res)); // Set OCR2A value (TOP) = res + } + else + _SET_ICRn(timer, res); // Set ICRn value (TOP) = res +} + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + // If v is 0 or v_size (max), digitalWrite to LOW or HIGH. + // Note that digitalWrite also disables PWM output for us (sets COM bit to 0) + if (v == 0) + digitalWrite(pin, invert); + else if (v == v_size) + digitalWrite(pin, !invert); + else { + const Timer timer = get_pwm_timer(pin); + if (timer.isPWM) { + if (timer.n == 0) { + _SET_COMnQ(timer, timer.q, COM_CLEAR_SET); // Only allow a TIMER0B select... + _SET_OCRnQ(timer, timer.q, v); // ...and OCR0B duty update. For output pin D4 no frequency changes are permitted. + } + else if (!timer.isProtected) { + const uint16_t top = timer.n == 2 ? TERN(USE_OCR2A_AS_TOP, *timer.OCRnQ[0], 255) : *timer.ICRn; + _SET_COMnQ(timer, SUM_TERN(HAS_TCCR2, timer.q, timer.q == 2), COM_CLEAR_SET + invert); // COM20 is on bit 4 of TCCR2, so +1 for q==2 + _SET_OCRnQ(timer, timer.q, uint16_t(uint32_t(v) * top / v_size)); // Scale 8/16-bit v to top value + } + } + else + digitalWrite(pin, v < v_size / 2 ? LOW : HIGH); + } +} + +void MarlinHAL::init_pwm_timers() { + // Init some timer frequencies to a default 1KHz + const pin_t pwm_pin[] = { + #ifdef __AVR_ATmega2560__ + 10, 5, 6, 46 + #elif defined(__AVR_ATmega1280__) + 12, 31 + #elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega1284__) + 15, 6 + #elif defined(__AVR_AT90USB1286__) || defined(__AVR_mega64) || defined(__AVR_mega128) + 16, 24 + #endif + }; + + LOOP_L_N(i, COUNT(pwm_pin)) + set_pwm_frequency(pwm_pin[i], 1000); +} + +#endif // __AVR__ diff --git a/src/HAL/AVR/fastio.cpp b/src/HAL/AVR/fastio.cpp new file mode 100644 index 0000000..5c6ef18 --- /dev/null +++ b/src/HAL/AVR/fastio.cpp @@ -0,0 +1,288 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Fast I/O for extended pins + */ + +#ifdef __AVR__ + +#include "fastio.h" + +#ifdef FASTIO_EXT_START + +#include "../shared/Marduino.h" + +#define _IS_EXT(P) WITHIN(P, FASTIO_EXT_START, FASTIO_EXT_END) + +void extDigitalWrite(const int8_t pin, const uint8_t state) { + #define _WCASE(N) case N: WRITE(N, state); break + switch (pin) { + default: digitalWrite(pin, state); + #if _IS_EXT(70) + _WCASE(70); + #endif + #if _IS_EXT(71) + _WCASE(71); + #endif + #if _IS_EXT(72) + _WCASE(72); + #endif + #if _IS_EXT(73) + _WCASE(73); + #endif + #if _IS_EXT(74) + _WCASE(74); + #endif + #if _IS_EXT(75) + _WCASE(75); + #endif + #if _IS_EXT(76) + _WCASE(76); + #endif + #if _IS_EXT(77) + _WCASE(77); + #endif + #if _IS_EXT(78) + _WCASE(78); + #endif + #if _IS_EXT(79) + _WCASE(79); + #endif + #if _IS_EXT(80) + _WCASE(80); + #endif + #if _IS_EXT(81) + _WCASE(81); + #endif + #if _IS_EXT(82) + _WCASE(82); + #endif + #if _IS_EXT(83) + _WCASE(83); + #endif + #if _IS_EXT(84) + _WCASE(84); + #endif + #if _IS_EXT(85) + _WCASE(85); + #endif + #if _IS_EXT(86) + _WCASE(86); + #endif + #if _IS_EXT(87) + _WCASE(87); + #endif + #if _IS_EXT(88) + _WCASE(88); + #endif + #if _IS_EXT(89) + _WCASE(89); + #endif + #if _IS_EXT(90) + _WCASE(90); + #endif + #if _IS_EXT(91) + _WCASE(91); + #endif + #if _IS_EXT(92) + _WCASE(92); + #endif + #if _IS_EXT(93) + _WCASE(93); + #endif + #if _IS_EXT(94) + _WCASE(94); + #endif + #if _IS_EXT(95) + _WCASE(95); + #endif + #if _IS_EXT(96) + _WCASE(96); + #endif + #if _IS_EXT(97) + _WCASE(97); + #endif + #if _IS_EXT(98) + _WCASE(98); + #endif + #if _IS_EXT(99) + _WCASE(99); + #endif + #if _IS_EXT(100) + _WCASE(100); + #endif + } +} + +uint8_t extDigitalRead(const int8_t pin) { + #define _RCASE(N) case N: return READ(N) + switch (pin) { + default: return digitalRead(pin); + #if _IS_EXT(70) + _RCASE(70); + #endif + #if _IS_EXT(71) + _RCASE(71); + #endif + #if _IS_EXT(72) + _RCASE(72); + #endif + #if _IS_EXT(73) + _RCASE(73); + #endif + #if _IS_EXT(74) + _RCASE(74); + #endif + #if _IS_EXT(75) + _RCASE(75); + #endif + #if _IS_EXT(76) + _RCASE(76); + #endif + #if _IS_EXT(77) + _RCASE(77); + #endif + #if _IS_EXT(78) + _RCASE(78); + #endif + #if _IS_EXT(79) + _RCASE(79); + #endif + #if _IS_EXT(80) + _RCASE(80); + #endif + #if _IS_EXT(81) + _RCASE(81); + #endif + #if _IS_EXT(82) + _RCASE(82); + #endif + #if _IS_EXT(83) + _RCASE(83); + #endif + #if _IS_EXT(84) + _RCASE(84); + #endif + #if _IS_EXT(85) + _RCASE(85); + #endif + #if _IS_EXT(86) + _RCASE(86); + #endif + #if _IS_EXT(87) + _RCASE(87); + #endif + #if _IS_EXT(88) + _RCASE(88); + #endif + #if _IS_EXT(89) + _RCASE(89); + #endif + #if _IS_EXT(90) + _RCASE(90); + #endif + #if _IS_EXT(91) + _RCASE(91); + #endif + #if _IS_EXT(92) + _RCASE(92); + #endif + #if _IS_EXT(93) + _RCASE(93); + #endif + #if _IS_EXT(94) + _RCASE(94); + #endif + #if _IS_EXT(95) + _RCASE(95); + #endif + #if _IS_EXT(96) + _RCASE(96); + #endif + #if _IS_EXT(97) + _RCASE(97); + #endif + #if _IS_EXT(98) + _RCASE(98); + #endif + #if _IS_EXT(99) + _RCASE(99); + #endif + #if _IS_EXT(100) + _RCASE(100); + #endif + } +} + +#if 0 +/** + * Set Timer 5 PWM frequency in Hz, from 3.8Hz up to ~16MHz + * with a minimum resolution of 100 steps. + * + * DC values -1.0 to 1.0. Negative duty cycle inverts the pulse. + */ +uint16_t set_pwm_frequency_hz(const_float_t hz, const float dca, const float dcb, const float dcc) { + float count = 0; + if (hz > 0 && (dca || dcb || dcc)) { + count = float(F_CPU) / hz; // 1x prescaler, TOP for 16MHz base freq. + uint16_t prescaler; // Range of 30.5Hz (65535) 64.5kHz (>31) + + if (count >= 255. * 256.) { prescaler = 1024; SET_CS(5, PRESCALER_1024); } + else if (count >= 255. * 64.) { prescaler = 256; SET_CS(5, PRESCALER_256); } + else if (count >= 255. * 8.) { prescaler = 64; SET_CS(5, PRESCALER_64); } + else if (count >= 255.) { prescaler = 8; SET_CS(5, PRESCALER_8); } + else { prescaler = 1; SET_CS(5, PRESCALER_1); } + + count /= float(prescaler); + const float pwm_top = round(count); // Get the rounded count + + ICR5 = (uint16_t)pwm_top - 1; // Subtract 1 for TOP + OCR5A = pwm_top * ABS(dca); // Update and scale DCs + OCR5B = pwm_top * ABS(dcb); + OCR5C = pwm_top * ABS(dcc); + _SET_COM(5, A, dca ? (dca < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); // Set compare modes + _SET_COM(5, B, dcb ? (dcb < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); + _SET_COM(5, C, dcc ? (dcc < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); + + SET_WGM(5, FAST_PWM_ICRn); // Fast PWM with ICR5 as TOP + + //SERIAL_ECHOLNPGM("Timer 5 Settings:"); + //SERIAL_ECHOLNPGM(" Prescaler=", prescaler); + //SERIAL_ECHOLNPGM(" TOP=", ICR5); + //SERIAL_ECHOLNPGM(" OCR5A=", OCR5A); + //SERIAL_ECHOLNPGM(" OCR5B=", OCR5B); + //SERIAL_ECHOLNPGM(" OCR5C=", OCR5C); + } + else { + // Restore the default for Timer 5 + SET_WGM(5, PWM_PC_8); // PWM 8-bit (Phase Correct) + SET_COMS(5, NORMAL, NORMAL, NORMAL); // Do nothing + SET_CS(5, PRESCALER_64); // 16MHz / 64 = 250kHz + OCR5A = OCR5B = OCR5C = 0; + } + return round(count); +} +#endif + +#endif // FASTIO_EXT_START +#endif // __AVR__ diff --git a/src/HAL/AVR/fastio.h b/src/HAL/AVR/fastio.h new file mode 100644 index 0000000..51d3b31 --- /dev/null +++ b/src/HAL/AVR/fastio.h @@ -0,0 +1,350 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for AVR + * Use direct port manipulation to save scads of processor time. + * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al. + */ + +#include + +#if defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB1286P__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB646P__) || defined(__AVR_AT90USB647__) + #define AVR_AT90USB1286_FAMILY 1 +#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__) + #define AVR_ATmega1284_FAMILY 1 +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #define AVR_ATmega2560_FAMILY 1 +#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + #define AVR_ATmega2561_FAMILY 1 +#elif defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) + #define AVR_ATmega328_FAMILY 1 +#endif + +/** + * Include Ports and Functions + */ +#if AVR_ATmega328_FAMILY + #include "fastio/fastio_168.h" +#elif AVR_ATmega1284_FAMILY + #include "fastio/fastio_644.h" +#elif AVR_ATmega2560_FAMILY + #include "fastio/fastio_1280.h" +#elif AVR_AT90USB1286_FAMILY + #include "fastio/fastio_AT90USB.h" +#elif AVR_ATmega2561_FAMILY + #include "fastio/fastio_1281.h" +#else + #error "No FastIO definition for the selected AVR Board." +#endif + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(PIN); WRITE(PIN, HIGH); WRITE(PIN, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/cpp/Stringification.html + */ + +#define _READ(IO) TEST(DIO ## IO ## _RPORT, DIO ## IO ## _PIN) + +#define _WRITE_NC(IO,V) do{ \ + if (V) SBI(DIO ## IO ## _WPORT, DIO ## IO ## _PIN); \ + else CBI(DIO ## IO ## _WPORT, DIO ## IO ## _PIN); \ +}while(0) + +#define _WRITE_C(IO,V) do{ \ + uint8_t port_bits = DIO ## IO ## _WPORT; /* Get a mask from the current port bits */ \ + if (V) port_bits = ~port_bits; /* For setting bits, invert the mask */ \ + DIO ## IO ## _RPORT = port_bits & _BV(DIO ## IO ## _PIN); /* Atomically toggle the output port bits */ \ +}while(0) + +#define _WRITE(IO,V) do{ if (&(DIO ## IO ## _RPORT) < (uint8_t*)0x100) _WRITE_NC(IO,V); else _WRITE_C(IO,V); }while(0) + +#define _TOGGLE(IO) (DIO ## IO ## _RPORT = _BV(DIO ## IO ## _PIN)) + +#define _SET_INPUT(IO) CBI(DIO ## IO ## _DDR, DIO ## IO ## _PIN) +#define _SET_OUTPUT(IO) SBI(DIO ## IO ## _DDR, DIO ## IO ## _PIN) + +#define _IS_INPUT(IO) !TEST(DIO ## IO ## _DDR, DIO ## IO ## _PIN) +#define _IS_OUTPUT(IO) TEST(DIO ## IO ## _DDR, DIO ## IO ## _PIN) + +// digitalRead/Write wrappers +#ifdef FASTIO_EXT_START + void extDigitalWrite(const int8_t pin, const uint8_t state); + uint8_t extDigitalRead(const int8_t pin); +#else + #define extDigitalWrite(IO,V) digitalWrite(IO,V) + #define extDigitalRead(IO) digitalRead(IO) +#endif + +#define READ(IO) _READ(IO) +#define WRITE(IO,V) _WRITE(IO,V) +#define TOGGLE(IO) _TOGGLE(IO) + +#define SET_INPUT(IO) _SET_INPUT(IO) +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _WRITE(IO, HIGH); }while(0) +#define SET_INPUT_PULLDOWN SET_INPUT +#define SET_OUTPUT(IO) _SET_OUTPUT(IO) +#define SET_PWM SET_OUTPUT + +#define IS_INPUT(IO) _IS_INPUT(IO) +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +/** + * Timer and Interrupt Control + */ + +// Waveform Generation Modes +enum WaveGenMode : uint8_t { + WGM_NORMAL, // 0 + WGM_PWM_PC_8, // 1 + WGM_PWM_PC_9, // 2 + WGM_PWM_PC_10, // 3 + WGM_CTC_OCRnA, // 4 COM OCnx + WGM_FAST_PWM_8, // 5 + WGM_FAST_PWM_9, // 6 + WGM_FAST_PWM_10, // 7 + WGM_PWM_PC_FC_ICRn, // 8 + WGM_PWM_PC_FC_OCRnA, // 9 COM OCnA + WGM_PWM_PC_ICRn, // 10 + WGM_PWM_PC_OCRnA, // 11 COM OCnA + WGM_CTC_ICRn, // 12 COM OCnx + WGM_reserved, // 13 + WGM_FAST_PWM_ICRn, // 14 COM OCnA + WGM_FAST_PWM_OCRnA // 15 COM OCnA +}; + +// Wavefore Generation Modes (Timer 2 only) +enum WaveGenMode2 : uint8_t { + WGM2_NORMAL, // 0 + WGM2_PWM_PC, // 1 + WGM2_CTC_OCR2A, // 2 + WGM2_FAST_PWM, // 3 + WGM2_reserved_1, // 4 + WGM2_PWM_PC_OCR2A, // 5 + WGM2_reserved_2, // 6 + WGM2_FAST_PWM_OCR2A, // 7 +}; + +// Compare Modes +enum CompareMode : uint8_t { + COM_NORMAL, // 0 + COM_TOGGLE, // 1 Non-PWM: OCnx ... Both PWM (WGM 9,11,14,15): OCnA only ... else NORMAL + COM_CLEAR_SET, // 2 Non-PWM: OCnx ... Fast PWM: OCnx/Bottom ... PF-FC: OCnx Up/Down + COM_SET_CLEAR // 3 Non-PWM: OCnx ... Fast PWM: OCnx/Bottom ... PF-FC: OCnx Up/Down +}; + +// Clock Sources +enum ClockSource : uint8_t { + CS_NONE, // 0 + CS_PRESCALER_1, // 1 + CS_PRESCALER_8, // 2 + CS_PRESCALER_64, // 3 + CS_PRESCALER_256, // 4 + CS_PRESCALER_1024, // 5 + CS_EXT_FALLING, // 6 + CS_EXT_RISING // 7 +}; + +// Clock Sources (Timer 2 only) +enum ClockSource2 : uint8_t { + CS2_NONE, // 0 + CS2_PRESCALER_1, // 1 + CS2_PRESCALER_8, // 2 + CS2_PRESCALER_32, // 3 + CS2_PRESCALER_64, // 4 + CS2_PRESCALER_128, // 5 + CS2_PRESCALER_256, // 6 + CS2_PRESCALER_1024 // 7 +}; + +// Get interrupt bits in an orderly way +// Ex: cs = GET_CS(0); coma1 = GET_COM(A,1); +#define GET_WGM(T) (((TCCR##T##A >> WGM##T##0) & 0x3) | ((TCCR##T##B >> WGM##T##2 << 2) & 0xC)) +#define GET_CS(T) ((TCCR##T##B >> CS##T##0) & 0x7) +#define GET_COM(T,Q) ((TCCR##T##Q >> COM##T##Q##0) & 0x3) +#define GET_COMA(T) GET_COM(T,A) +#define GET_COMB(T) GET_COM(T,B) +#define GET_COMC(T) GET_COM(T,C) +#define GET_ICNC(T) (!!(TCCR##T##B & _BV(ICNC##T))) +#define GET_ICES(T) (!!(TCCR##T##B & _BV(ICES##T))) +#define GET_FOC(T,Q) (!!(TCCR##T##C & _BV(FOC##T##Q))) +#define GET_FOCA(T) GET_FOC(T,A) +#define GET_FOCB(T) GET_FOC(T,B) +#define GET_FOCC(T) GET_FOC(T,C) + +// Set Wave Generation Mode bits +// Ex: SET_WGM(5,CTC_ICRn); +#define _SET_WGM(T,V) do{ \ + TCCR##T##A = (TCCR##T##A & ~(0x3 << WGM##T##0)) | (( int(V) & 0x3) << WGM##T##0); \ + TCCR##T##B = (TCCR##T##B & ~(0x3 << WGM##T##2)) | (((int(V) >> 2) & 0x3) << WGM##T##2); \ + }while(0) +#define SET_WGM(T,V) _SET_WGM(T,WGM_##V) + +// Set Clock Select bits +// Ex: SET_CS3(PRESCALER_64); +#ifdef TCCR2 + #define HAS_TCCR2 1 +#endif +#define _SET_CS(T,V) (TCCR##T##B = (TCCR##T##B & ~(0x7 << CS##T##0)) | ((int(V) & 0x7) << CS##T##0)) +#define _SET_CS0(V) _SET_CS(0,V) +#define _SET_CS1(V) _SET_CS(1,V) +#define _SET_CS3(V) _SET_CS(3,V) +#define _SET_CS4(V) _SET_CS(4,V) +#define _SET_CS5(V) _SET_CS(5,V) +#define SET_CS0(V) _SET_CS0(CS_##V) +#define SET_CS1(V) _SET_CS1(CS_##V) + +#if HAS_TCCR2 + #define _SET_CS2(V) (TCCR2 = (TCCR2 & ~(0x7 << CS20)) | (int(V) << CS20)) + #define SET_CS2(V) _SET_CS2(CS2_##V) +#else + #define _SET_CS2(V) _SET_CS(2,V) + #define SET_CS2(V) _SET_CS2(CS_##V) +#endif + +#define SET_CS3(V) _SET_CS3(CS_##V) +#define SET_CS4(V) _SET_CS4(CS_##V) +#define SET_CS5(V) _SET_CS5(CS_##V) +#define SET_CS(T,V) SET_CS##T(V) + +// Set Compare Mode bits +// Ex: SET_COMS(4,CLEAR_SET,CLEAR_SET,CLEAR_SET); +#define _SET_COM(T,Q,V) (TCCR##T##Q = (TCCR##T##Q & ~(0x3 << COM##T##Q##0)) | (int(V) << COM##T##Q##0)) +#define SET_COM(T,Q,V) _SET_COM(T,Q,COM_##V) +#define SET_COMA(T,V) SET_COM(T,A,V) +#define SET_COMB(T,V) SET_COM(T,B,V) +#define SET_COMC(T,V) SET_COM(T,C,V) +#define SET_COMS(T,V1,V2,V3) do{ SET_COMA(T,V1); SET_COMB(T,V2); SET_COMC(T,V3); }while(0) + +// Set Noise Canceler bit +// Ex: SET_ICNC(2,1) +#define SET_ICNC(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICNC##T) : TCCR##T##B & ~_BV(ICNC##T)) + +// Set Input Capture Edge Select bit +// Ex: SET_ICES(5,0) +#define SET_ICES(T,V) (TCCR##T##B = (V) ? TCCR##T##B | _BV(ICES##T) : TCCR##T##B & ~_BV(ICES##T)) + +// Set Force Output Compare bit +// Ex: SET_FOC(3,A,1) +#define SET_FOC(T,Q,V) (TCCR##T##C = (V) ? TCCR##T##C | _BV(FOC##T##Q) : TCCR##T##C & ~_BV(FOC##T##Q)) +#define SET_FOCA(T,V) SET_FOC(T,A,V) +#define SET_FOCB(T,V) SET_FOC(T,B,V) +#define SET_FOCC(T,V) SET_FOC(T,C,V) + +#if 0 + +/** + * PWM availability macros + */ + +// Determine which hardware PWMs are already in use +#define _PWM_CHK_FAN_B(P) (P == E0_AUTO_FAN_PIN || P == E1_AUTO_FAN_PIN || P == E2_AUTO_FAN_PIN || P == E3_AUTO_FAN_PIN || P == E4_AUTO_FAN_PIN || P == E5_AUTO_FAN_PIN || P == E6_AUTO_FAN_PIN || P == E7_AUTO_FAN_PIN || P == CHAMBER_AUTO_FAN_PIN || P == COOLER_AUTO_FAN_PIN) +#if PIN_EXISTS(CONTROLLER_FAN) + #define PWM_CHK_FAN_B(P) (_PWM_CHK_FAN_B(P) || P == CONTROLLER_FAN_PIN) +#else + #define PWM_CHK_FAN_B(P) _PWM_CHK_FAN_B(P) +#endif + +#if ANY_PIN(FAN, FAN1, FAN2, FAN3, FAN4, FAN5, FAN6, FAN7) + #if PIN_EXISTS(FAN7) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN || P == FAN6_PIN || P == FAN7_PIN) + #elif PIN_EXISTS(FAN6) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN || P == FAN6_PIN) + #elif PIN_EXISTS(FAN5) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN || P == FAN5_PIN) + #elif PIN_EXISTS(FAN4) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN || P == FAN4_PIN) + #elif PIN_EXISTS(FAN3) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN || P == FAN3_PIN) + #elif PIN_EXISTS(FAN2) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN || P == FAN2_PIN) + #elif PIN_EXISTS(FAN1) + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN || P == FAN1_PIN) + #else + #define PWM_CHK_FAN_A(P) (P == FAN0_PIN) + #endif +#else + #define PWM_CHK_FAN_A(P) false +#endif + +#if HAS_MOTOR_CURRENT_PWM + #if PIN_EXISTS(MOTOR_CURRENT_PWM_XY) + #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E || P == MOTOR_CURRENT_PWM_Z || P == MOTOR_CURRENT_PWM_XY) + #elif PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E || P == MOTOR_CURRENT_PWM_Z) + #else + #define PWM_CHK_MOTOR_CURRENT(P) (P == MOTOR_CURRENT_PWM_E) + #endif +#else + #define PWM_CHK_MOTOR_CURRENT(P) false +#endif + +#ifdef NUM_SERVOS + #if AVR_ATmega2560_FAMILY + #define PWM_CHK_SERVO(P) (P == 5 || (NUM_SERVOS > 12 && P == 6) || (NUM_SERVOS > 24 && P == 46)) // PWMS 3A, 4A & 5A + #elif AVR_ATmega2561_FAMILY + #define PWM_CHK_SERVO(P) (P == 5) // PWM3A + #elif AVR_ATmega1284_FAMILY + #define PWM_CHK_SERVO(P) false + #elif AVR_AT90USB1286_FAMILY + #define PWM_CHK_SERVO(P) (P == 16) // PWM3A + #elif AVR_ATmega328_FAMILY + #define PWM_CHK_SERVO(P) false + #endif +#else + #define PWM_CHK_SERVO(P) false +#endif + +#if ENABLED(BARICUDA) + #if HAS_HEATER_1 && HAS_HEATER_2 + #define PWM_CHK_HEATER(P) (P == HEATER_1_PIN || P == HEATER_2_PIN) + #elif HAS_HEATER_1 + #define PWM_CHK_HEATER(P) (P == HEATER_1_PIN) + #endif +#else + #define PWM_CHK_HEATER(P) false +#endif + +#define PWM_CHK(P) (PWM_CHK_HEATER(P) || PWM_CHK_SERVO(P) || PWM_CHK_MOTOR_CURRENT(P) || PWM_CHK_FAN_A(P) || PWM_CHK_FAN_B(P)) + +#endif // PWM_CHK is not used in Marlin + +// define which hardware PWMs are available for the current CPU +// all timer 1 PWMS deleted from this list because they are never available +#if AVR_ATmega2560_FAMILY + #define PWM_PIN(P) ((P >= 2 && P <= 10) || P == 13 || P == 44 || P == 45 || P == 46) +#elif AVR_ATmega2561_FAMILY + #define PWM_PIN(P) ((P >= 2 && P <= 6) || P == 9) +#elif AVR_ATmega1284_FAMILY + #define PWM_PIN(P) (P == 3 || P == 4 || P == 14 || P == 15) +#elif AVR_AT90USB1286_FAMILY + #define PWM_PIN(P) (P == 0 || P == 1 || P == 14 || P == 15 || P == 16 || P == 24) +#elif AVR_ATmega328_FAMILY + #define PWM_PIN(P) (P == 3 || P == 5 || P == 6 || P == 11) +#else + #error "unknown CPU" +#endif diff --git a/src/HAL/AVR/fastio/fastio_1280.h b/src/HAL/AVR/fastio/fastio_1280.h new file mode 100644 index 0000000..f482f82 --- /dev/null +++ b/src/HAL/AVR/fastio/fastio_1280.h @@ -0,0 +1,1114 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Pin mapping for the 1280 and 2560 + * + * Hardware Pin : 02 03 06 07 01 05 15 16 17 18 23 24 25 26 64 63 13 12 46 45 44 43 78 77 76 75 74 73 72 71 60 59 58 57 56 55 54 53 50 70 52 51 42 41 40 39 38 37 36 35 22 21 20 19 97 96 95 94 93 92 91 90 89 88 87 86 85 84 83 82 | 04 08 09 10 11 14 27 28 29 30 31 32 33 34 47 48 49 61 62 65 66 67 68 69 79 80 81 98 99 100 + * Port : E0 E1 E4 E5 G5 E3 H3 H4 H5 H6 B4 B5 B6 B7 J1 J0 H1 H0 D3 D2 D1 D0 A0 A1 A2 A3 A4 A5 A6 A7 C7 C6 C5 C4 C3 C2 C1 C0 D7 G2 G1 G0 L7 L6 L5 L4 L3 L2 L1 L0 B3 B2 B1 B0 F0 F1 F2 F3 F4 F5 F6 F7 K0 K1 K2 K3 K4 K5 K6 K7 | E2 E6 E7 xx xx H2 H7 G3 G4 xx xx xx xx xx D4 D5 D6 xx xx J2 J3 J4 J5 J6 J7 xx xx xx xx xx + * Logical Pin : 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 | 78 79 80 xx xx 84 85 71 70 xx xx xx xx xx 81 82 83 xx xx 72 73 75 76 77 74 xx xx xx xx xx + */ + +#include "../fastio.h" + +// change for your board +#define DEBUG_LED DIO21 + +// UART +#define RXD DIO0 +#define TXD DIO1 + +// SPI +#define SCK DIO52 +#define MISO DIO50 +#define MOSI DIO51 +#define SS DIO53 + +// TWI (I2C) +#define SCL DIO21 +#define SDA DIO20 + +// Timers and PWM +#define OC0A DIO13 +#define OC0B DIO4 +#define OC1A DIO11 +#define OC1B DIO12 +#define OC2A DIO10 +#define OC2B DIO9 +#define OC3A DIO5 +#define OC3B DIO2 +#define OC3C DIO3 +#define OC4A DIO6 +#define OC4B DIO7 +#define OC4C DIO8 +#define OC5A DIO46 +#define OC5B DIO45 +#define OC5C DIO44 + +// Digital I/O + +#define DIO0_PIN PINE0 +#define DIO0_RPORT PINE +#define DIO0_WPORT PORTE +#define DIO0_DDR DDRE +#define DIO0_PWM nullptr + +#define DIO1_PIN PINE1 +#define DIO1_RPORT PINE +#define DIO1_WPORT PORTE +#define DIO1_DDR DDRE +#define DIO1_PWM nullptr + +#define DIO2_PIN PINE4 +#define DIO2_RPORT PINE +#define DIO2_WPORT PORTE +#define DIO2_DDR DDRE +#define DIO2_PWM &OCR3BL + +#define DIO3_PIN PINE5 +#define DIO3_RPORT PINE +#define DIO3_WPORT PORTE +#define DIO3_DDR DDRE +#define DIO3_PWM &OCR3CL + +#define DIO4_PIN PING5 +#define DIO4_RPORT PING +#define DIO4_WPORT PORTG +#define DIO4_DDR DDRG +#define DIO4_PWM &OCR0B + +#define DIO5_PIN PINE3 +#define DIO5_RPORT PINE +#define DIO5_WPORT PORTE +#define DIO5_DDR DDRE +#define DIO5_PWM &OCR3AL + +#define DIO6_PIN PINH3 +#define DIO6_RPORT PINH +#define DIO6_WPORT PORTH +#define DIO6_DDR DDRH +#define DIO6_PWM &OCR4AL + +#define DIO7_PIN PINH4 +#define DIO7_RPORT PINH +#define DIO7_WPORT PORTH +#define DIO7_DDR DDRH +#define DIO7_PWM &OCR4BL + +#define DIO8_PIN PINH5 +#define DIO8_RPORT PINH +#define DIO8_WPORT PORTH +#define DIO8_DDR DDRH +#define DIO8_PWM &OCR4CL + +#define DIO9_PIN PINH6 +#define DIO9_RPORT PINH +#define DIO9_WPORT PORTH +#define DIO9_DDR DDRH +#define DIO9_PWM &OCR2B + +#define DIO10_PIN PINB4 +#define DIO10_RPORT PINB +#define DIO10_WPORT PORTB +#define DIO10_DDR DDRB +#define DIO10_PWM &OCR2A + +#define DIO11_PIN PINB5 +#define DIO11_RPORT PINB +#define DIO11_WPORT PORTB +#define DIO11_DDR DDRB +#define DIO11_PWM nullptr + +#define DIO12_PIN PINB6 +#define DIO12_RPORT PINB +#define DIO12_WPORT PORTB +#define DIO12_DDR DDRB +#define DIO12_PWM nullptr + +#define DIO13_PIN PINB7 +#define DIO13_RPORT PINB +#define DIO13_WPORT PORTB +#define DIO13_DDR DDRB +#define DIO13_PWM &OCR0A + +#define DIO14_PIN PINJ1 +#define DIO14_RPORT PINJ +#define DIO14_WPORT PORTJ +#define DIO14_DDR DDRJ +#define DIO14_PWM nullptr + +#define DIO15_PIN PINJ0 +#define DIO15_RPORT PINJ +#define DIO15_WPORT PORTJ +#define DIO15_DDR DDRJ +#define DIO15_PWM nullptr + +#define DIO16_PIN PINH1 +#define DIO16_RPORT PINH +#define DIO16_WPORT PORTH +#define DIO16_DDR DDRH +#define DIO16_PWM nullptr + +#define DIO17_PIN PINH0 +#define DIO17_RPORT PINH +#define DIO17_WPORT PORTH +#define DIO17_DDR DDRH +#define DIO17_PWM nullptr + +#define DIO18_PIN PIND3 +#define DIO18_RPORT PIND +#define DIO18_WPORT PORTD +#define DIO18_DDR DDRD +#define DIO18_PWM nullptr + +#define DIO19_PIN PIND2 +#define DIO19_RPORT PIND +#define DIO19_WPORT PORTD +#define DIO19_DDR DDRD +#define DIO19_PWM nullptr + +#define DIO20_PIN PIND1 +#define DIO20_RPORT PIND +#define DIO20_WPORT PORTD +#define DIO20_DDR DDRD +#define DIO20_PWM nullptr + +#define DIO21_PIN PIND0 +#define DIO21_RPORT PIND +#define DIO21_WPORT PORTD +#define DIO21_DDR DDRD +#define DIO21_PWM nullptr + +#define DIO22_PIN PINA0 +#define DIO22_RPORT PINA +#define DIO22_WPORT PORTA +#define DIO22_DDR DDRA +#define DIO22_PWM nullptr + +#define DIO23_PIN PINA1 +#define DIO23_RPORT PINA +#define DIO23_WPORT PORTA +#define DIO23_DDR DDRA +#define DIO23_PWM nullptr + +#define DIO24_PIN PINA2 +#define DIO24_RPORT PINA +#define DIO24_WPORT PORTA +#define DIO24_DDR DDRA +#define DIO24_PWM nullptr + +#define DIO25_PIN PINA3 +#define DIO25_RPORT PINA +#define DIO25_WPORT PORTA +#define DIO25_DDR DDRA +#define DIO25_PWM nullptr + +#define DIO26_PIN PINA4 +#define DIO26_RPORT PINA +#define DIO26_WPORT PORTA +#define DIO26_DDR DDRA +#define DIO26_PWM nullptr + +#define DIO27_PIN PINA5 +#define DIO27_RPORT PINA +#define DIO27_WPORT PORTA +#define DIO27_DDR DDRA +#define DIO27_PWM nullptr + +#define DIO28_PIN PINA6 +#define DIO28_RPORT PINA +#define DIO28_WPORT PORTA +#define DIO28_DDR DDRA +#define DIO28_PWM nullptr + +#define DIO29_PIN PINA7 +#define DIO29_RPORT PINA +#define DIO29_WPORT PORTA +#define DIO29_DDR DDRA +#define DIO29_PWM nullptr + +#define DIO30_PIN PINC7 +#define DIO30_RPORT PINC +#define DIO30_WPORT PORTC +#define DIO30_DDR DDRC +#define DIO30_PWM nullptr + +#define DIO31_PIN PINC6 +#define DIO31_RPORT PINC +#define DIO31_WPORT PORTC +#define DIO31_DDR DDRC +#define DIO31_PWM nullptr + +#define DIO32_PIN PINC5 +#define DIO32_RPORT PINC +#define DIO32_WPORT PORTC +#define DIO32_DDR DDRC +#define DIO32_PWM nullptr + +#define DIO33_PIN PINC4 +#define DIO33_RPORT PINC +#define DIO33_WPORT PORTC +#define DIO33_DDR DDRC +#define DIO33_PWM nullptr + +#define DIO34_PIN PINC3 +#define DIO34_RPORT PINC +#define DIO34_WPORT PORTC +#define DIO34_DDR DDRC +#define DIO34_PWM nullptr + +#define DIO35_PIN PINC2 +#define DIO35_RPORT PINC +#define DIO35_WPORT PORTC +#define DIO35_DDR DDRC +#define DIO35_PWM nullptr + +#define DIO36_PIN PINC1 +#define DIO36_RPORT PINC +#define DIO36_WPORT PORTC +#define DIO36_DDR DDRC +#define DIO36_PWM nullptr + +#define DIO37_PIN PINC0 +#define DIO37_RPORT PINC +#define DIO37_WPORT PORTC +#define DIO37_DDR DDRC +#define DIO37_PWM nullptr + +#define DIO38_PIN PIND7 +#define DIO38_RPORT PIND +#define DIO38_WPORT PORTD +#define DIO38_DDR DDRD +#define DIO38_PWM nullptr + +#define DIO39_PIN PING2 +#define DIO39_RPORT PING +#define DIO39_WPORT PORTG +#define DIO39_DDR DDRG +#define DIO39_PWM nullptr + +#define DIO40_PIN PING1 +#define DIO40_RPORT PING +#define DIO40_WPORT PORTG +#define DIO40_DDR DDRG +#define DIO40_PWM nullptr + +#define DIO41_PIN PING0 +#define DIO41_RPORT PING +#define DIO41_WPORT PORTG +#define DIO41_DDR DDRG +#define DIO41_PWM nullptr + +#define DIO42_PIN PINL7 +#define DIO42_RPORT PINL +#define DIO42_WPORT PORTL +#define DIO42_DDR DDRL +#define DIO42_PWM nullptr + +#define DIO43_PIN PINL6 +#define DIO43_RPORT PINL +#define DIO43_WPORT PORTL +#define DIO43_DDR DDRL +#define DIO43_PWM nullptr + +#define DIO44_PIN PINL5 +#define DIO44_RPORT PINL +#define DIO44_WPORT PORTL +#define DIO44_DDR DDRL +#define DIO44_PWM &OCR5CL + +#define DIO45_PIN PINL4 +#define DIO45_RPORT PINL +#define DIO45_WPORT PORTL +#define DIO45_DDR DDRL +#define DIO45_PWM &OCR5BL + +#define DIO46_PIN PINL3 +#define DIO46_RPORT PINL +#define DIO46_WPORT PORTL +#define DIO46_DDR DDRL +#define DIO46_PWM &OCR5AL + +#define DIO47_PIN PINL2 +#define DIO47_RPORT PINL +#define DIO47_WPORT PORTL +#define DIO47_DDR DDRL +#define DIO47_PWM nullptr + +#define DIO48_PIN PINL1 +#define DIO48_RPORT PINL +#define DIO48_WPORT PORTL +#define DIO48_DDR DDRL +#define DIO48_PWM nullptr + +#define DIO49_PIN PINL0 +#define DIO49_RPORT PINL +#define DIO49_WPORT PORTL +#define DIO49_DDR DDRL +#define DIO49_PWM nullptr + +#define DIO50_PIN PINB3 +#define DIO50_RPORT PINB +#define DIO50_WPORT PORTB +#define DIO50_DDR DDRB +#define DIO50_PWM nullptr + +#define DIO51_PIN PINB2 +#define DIO51_RPORT PINB +#define DIO51_WPORT PORTB +#define DIO51_DDR DDRB +#define DIO51_PWM nullptr + +#define DIO52_PIN PINB1 +#define DIO52_RPORT PINB +#define DIO52_WPORT PORTB +#define DIO52_DDR DDRB +#define DIO52_PWM nullptr + +#define DIO53_PIN PINB0 +#define DIO53_RPORT PINB +#define DIO53_WPORT PORTB +#define DIO53_DDR DDRB +#define DIO53_PWM nullptr + +#define DIO54_PIN PINF0 +#define DIO54_RPORT PINF +#define DIO54_WPORT PORTF +#define DIO54_DDR DDRF +#define DIO54_PWM nullptr + +#define DIO55_PIN PINF1 +#define DIO55_RPORT PINF +#define DIO55_WPORT PORTF +#define DIO55_DDR DDRF +#define DIO55_PWM nullptr + +#define DIO56_PIN PINF2 +#define DIO56_RPORT PINF +#define DIO56_WPORT PORTF +#define DIO56_DDR DDRF +#define DIO56_PWM nullptr + +#define DIO57_PIN PINF3 +#define DIO57_RPORT PINF +#define DIO57_WPORT PORTF +#define DIO57_DDR DDRF +#define DIO57_PWM nullptr + +#define DIO58_PIN PINF4 +#define DIO58_RPORT PINF +#define DIO58_WPORT PORTF +#define DIO58_DDR DDRF +#define DIO58_PWM nullptr + +#define DIO59_PIN PINF5 +#define DIO59_RPORT PINF +#define DIO59_WPORT PORTF +#define DIO59_DDR DDRF +#define DIO59_PWM nullptr + +#define DIO60_PIN PINF6 +#define DIO60_RPORT PINF +#define DIO60_WPORT PORTF +#define DIO60_DDR DDRF +#define DIO60_PWM nullptr + +#define DIO61_PIN PINF7 +#define DIO61_RPORT PINF +#define DIO61_WPORT PORTF +#define DIO61_DDR DDRF +#define DIO61_PWM nullptr + +#define DIO62_PIN PINK0 +#define DIO62_RPORT PINK +#define DIO62_WPORT PORTK +#define DIO62_DDR DDRK +#define DIO62_PWM nullptr + +#define DIO63_PIN PINK1 +#define DIO63_RPORT PINK +#define DIO63_WPORT PORTK +#define DIO63_DDR DDRK +#define DIO63_PWM nullptr + +#define DIO64_PIN PINK2 +#define DIO64_RPORT PINK +#define DIO64_WPORT PORTK +#define DIO64_DDR DDRK +#define DIO64_PWM nullptr + +#define DIO65_PIN PINK3 +#define DIO65_RPORT PINK +#define DIO65_WPORT PORTK +#define DIO65_DDR DDRK +#define DIO65_PWM nullptr + +#define DIO66_PIN PINK4 +#define DIO66_RPORT PINK +#define DIO66_WPORT PORTK +#define DIO66_DDR DDRK +#define DIO66_PWM nullptr + +#define DIO67_PIN PINK5 +#define DIO67_RPORT PINK +#define DIO67_WPORT PORTK +#define DIO67_DDR DDRK +#define DIO67_PWM nullptr + +#define DIO68_PIN PINK6 +#define DIO68_RPORT PINK +#define DIO68_WPORT PORTK +#define DIO68_DDR DDRK +#define DIO68_PWM nullptr + +#define DIO69_PIN PINK7 +#define DIO69_RPORT PINK +#define DIO69_WPORT PORTK +#define DIO69_DDR DDRK +#define DIO69_PWM nullptr + +//#define FASTIO_EXT_START 70 +//#define FASTIO_EXT_END 85 + +#define DIO70_PIN PING4 +#define DIO70_RPORT PING +#define DIO70_WPORT PORTG +#define DIO70_DDR DDRG +#define DIO70_PWM nullptr + +#define DIO71_PIN PING3 +#define DIO71_RPORT PING +#define DIO71_WPORT PORTG +#define DIO71_DDR DDRG +#define DIO71_PWM nullptr + +#define DIO72_PIN PINJ2 +#define DIO72_RPORT PINJ +#define DIO72_WPORT PORTJ +#define DIO72_DDR DDRJ +#define DIO72_PWM nullptr + +#define DIO73_PIN PINJ3 +#define DIO73_RPORT PINJ +#define DIO73_WPORT PORTJ +#define DIO73_DDR DDRJ +#define DIO73_PWM nullptr + +#define DIO74_PIN PINJ7 +#define DIO74_RPORT PINJ +#define DIO74_WPORT PORTJ +#define DIO74_DDR DDRJ +#define DIO74_PWM nullptr + +#define DIO75_PIN PINJ4 +#define DIO75_RPORT PINJ +#define DIO75_WPORT PORTJ +#define DIO75_DDR DDRJ +#define DIO75_PWM nullptr + +#define DIO76_PIN PINJ5 +#define DIO76_RPORT PINJ +#define DIO76_WPORT PORTJ +#define DIO76_DDR DDRJ +#define DIO76_PWM nullptr + +#define DIO77_PIN PINJ6 +#define DIO77_RPORT PINJ +#define DIO77_WPORT PORTJ +#define DIO77_DDR DDRJ +#define DIO77_PWM nullptr + +#define DIO78_PIN PINE2 +#define DIO78_RPORT PINE +#define DIO78_WPORT PORTE +#define DIO78_DDR DDRE +#define DIO78_PWM nullptr + +#define DIO79_PIN PINE6 +#define DIO79_RPORT PINE +#define DIO79_WPORT PORTE +#define DIO79_DDR DDRE +#define DIO79_PWM nullptr + +#define DIO80_PIN PINE7 +#define DIO80_RPORT PINE +#define DIO80_WPORT PORTE +#define DIO80_DDR DDRE +#define DIO80_PWM nullptr + +#define DIO81_PIN PIND4 +#define DIO81_RPORT PIND +#define DIO81_WPORT PORTD +#define DIO81_DDR DDRD +#define DIO81_PWM nullptr + +#define DIO82_PIN PIND5 +#define DIO82_RPORT PIND +#define DIO82_WPORT PORTD +#define DIO82_DDR DDRD +#define DIO82_PWM nullptr + +#define DIO83_PIN PIND6 +#define DIO83_RPORT PIND +#define DIO83_WPORT PORTD +#define DIO83_DDR DDRD +#define DIO83_PWM nullptr + +#define DIO84_PIN PINH2 +#define DIO84_RPORT PINH +#define DIO84_WPORT PORTH +#define DIO84_DDR DDRH +#define DIO84_PWM nullptr + +#define DIO85_PIN PINH7 +#define DIO85_RPORT PINH +#define DIO85_WPORT PORTH +#define DIO85_DDR DDRH +#define DIO85_PWM nullptr + +#undef PA0 +#define PA0_PIN PINA0 +#define PA0_RPORT PINA +#define PA0_WPORT PORTA +#define PA0_DDR DDRA +#define PA0_PWM nullptr +#undef PA1 +#define PA1_PIN PINA1 +#define PA1_RPORT PINA +#define PA1_WPORT PORTA +#define PA1_DDR DDRA +#define PA1_PWM nullptr +#undef PA2 +#define PA2_PIN PINA2 +#define PA2_RPORT PINA +#define PA2_WPORT PORTA +#define PA2_DDR DDRA +#define PA2_PWM nullptr +#undef PA3 +#define PA3_PIN PINA3 +#define PA3_RPORT PINA +#define PA3_WPORT PORTA +#define PA3_DDR DDRA +#define PA3_PWM nullptr +#undef PA4 +#define PA4_PIN PINA4 +#define PA4_RPORT PINA +#define PA4_WPORT PORTA +#define PA4_DDR DDRA +#define PA4_PWM nullptr +#undef PA5 +#define PA5_PIN PINA5 +#define PA5_RPORT PINA +#define PA5_WPORT PORTA +#define PA5_DDR DDRA +#define PA5_PWM nullptr +#undef PA6 +#define PA6_PIN PINA6 +#define PA6_RPORT PINA +#define PA6_WPORT PORTA +#define PA6_DDR DDRA +#define PA6_PWM nullptr +#undef PA7 +#define PA7_PIN PINA7 +#define PA7_RPORT PINA +#define PA7_WPORT PORTA +#define PA7_DDR DDRA +#define PA7_PWM nullptr + +#undef PB0 +#define PB0_PIN PINB0 +#define PB0_RPORT PINB +#define PB0_WPORT PORTB +#define PB0_DDR DDRB +#define PB0_PWM nullptr +#undef PB1 +#define PB1_PIN PINB1 +#define PB1_RPORT PINB +#define PB1_WPORT PORTB +#define PB1_DDR DDRB +#define PB1_PWM nullptr +#undef PB2 +#define PB2_PIN PINB2 +#define PB2_RPORT PINB +#define PB2_WPORT PORTB +#define PB2_DDR DDRB +#define PB2_PWM nullptr +#undef PB3 +#define PB3_PIN PINB3 +#define PB3_RPORT PINB +#define PB3_WPORT PORTB +#define PB3_DDR DDRB +#define PB3_PWM nullptr +#undef PB4 +#define PB4_PIN PINB4 +#define PB4_RPORT PINB +#define PB4_WPORT PORTB +#define PB4_DDR DDRB +#define PB4_PWM &OCR2A +#undef PB5 +#define PB5_PIN PINB5 +#define PB5_RPORT PINB +#define PB5_WPORT PORTB +#define PB5_DDR DDRB +#define PB5_PWM nullptr +#undef PB6 +#define PB6_PIN PINB6 +#define PB6_RPORT PINB +#define PB6_WPORT PORTB +#define PB6_DDR DDRB +#define PB6_PWM nullptr +#undef PB7 +#define PB7_PIN PINB7 +#define PB7_RPORT PINB +#define PB7_WPORT PORTB +#define PB7_DDR DDRB +#define PB7_PWM &OCR0A + +#undef PC0 +#define PC0_PIN PINC0 +#define PC0_RPORT PINC +#define PC0_WPORT PORTC +#define PC0_DDR DDRC +#define PC0_PWM nullptr +#undef PC1 +#define PC1_PIN PINC1 +#define PC1_RPORT PINC +#define PC1_WPORT PORTC +#define PC1_DDR DDRC +#define PC1_PWM nullptr +#undef PC2 +#define PC2_PIN PINC2 +#define PC2_RPORT PINC +#define PC2_WPORT PORTC +#define PC2_DDR DDRC +#define PC2_PWM nullptr +#undef PC3 +#define PC3_PIN PINC3 +#define PC3_RPORT PINC +#define PC3_WPORT PORTC +#define PC3_DDR DDRC +#define PC3_PWM nullptr +#undef PC4 +#define PC4_PIN PINC4 +#define PC4_RPORT PINC +#define PC4_WPORT PORTC +#define PC4_DDR DDRC +#define PC4_PWM nullptr +#undef PC5 +#define PC5_PIN PINC5 +#define PC5_RPORT PINC +#define PC5_WPORT PORTC +#define PC5_DDR DDRC +#define PC5_PWM nullptr +#undef PC6 +#define PC6_PIN PINC6 +#define PC6_RPORT PINC +#define PC6_WPORT PORTC +#define PC6_DDR DDRC +#define PC6_PWM nullptr +#undef PC7 +#define PC7_PIN PINC7 +#define PC7_RPORT PINC +#define PC7_WPORT PORTC +#define PC7_DDR DDRC +#define PC7_PWM nullptr + +#undef PD0 +#define PD0_PIN PIND0 +#define PD0_RPORT PIND +#define PD0_WPORT PORTD +#define PD0_DDR DDRD +#define PD0_PWM nullptr +#undef PD1 +#define PD1_PIN PIND1 +#define PD1_RPORT PIND +#define PD1_WPORT PORTD +#define PD1_DDR DDRD +#define PD1_PWM nullptr +#undef PD2 +#define PD2_PIN PIND2 +#define PD2_RPORT PIND +#define PD2_WPORT PORTD +#define PD2_DDR DDRD +#define PD2_PWM nullptr +#undef PD3 +#define PD3_PIN PIND3 +#define PD3_RPORT PIND +#define PD3_WPORT PORTD +#define PD3_DDR DDRD +#define PD3_PWM nullptr +#undef PD4 +#define PD4_PIN PIND4 +#define PD4_RPORT PIND +#define PD4_WPORT PORTD +#define PD4_DDR DDRD +#define PD4_PWM nullptr +#undef PD5 +#define PD5_PIN PIND5 +#define PD5_RPORT PIND +#define PD5_WPORT PORTD +#define PD5_DDR DDRD +#define PD5_PWM nullptr +#undef PD6 +#define PD6_PIN PIND6 +#define PD6_RPORT PIND +#define PD6_WPORT PORTD +#define PD6_DDR DDRD +#define PD6_PWM nullptr +#undef PD7 +#define PD7_PIN PIND7 +#define PD7_RPORT PIND +#define PD7_WPORT PORTD +#define PD7_DDR DDRD +#define PD7_PWM nullptr + +#undef PE0 +#define PE0_PIN PINE0 +#define PE0_RPORT PINE +#define PE0_WPORT PORTE +#define PE0_DDR DDRE +#define PE0_PWM nullptr +#undef PE1 +#define PE1_PIN PINE1 +#define PE1_RPORT PINE +#define PE1_WPORT PORTE +#define PE1_DDR DDRE +#define PE1_PWM nullptr +#undef PE2 +#define PE2_PIN PINE2 +#define PE2_RPORT PINE +#define PE2_WPORT PORTE +#define PE2_DDR DDRE +#define PE2_PWM nullptr +#undef PE3 +#define PE3_PIN PINE3 +#define PE3_RPORT PINE +#define PE3_WPORT PORTE +#define PE3_DDR DDRE +#define PE3_PWM &OCR3AL +#undef PE4 +#define PE4_PIN PINE4 +#define PE4_RPORT PINE +#define PE4_WPORT PORTE +#define PE4_DDR DDRE +#define PE4_PWM &OCR3BL +#undef PE5 +#define PE5_PIN PINE5 +#define PE5_RPORT PINE +#define PE5_WPORT PORTE +#define PE5_DDR DDRE +#define PE5_PWM &OCR3CL +#undef PE6 +#define PE6_PIN PINE6 +#define PE6_RPORT PINE +#define PE6_WPORT PORTE +#define PE6_DDR DDRE +#define PE6_PWM nullptr +#undef PE7 +#define PE7_PIN PINE7 +#define PE7_RPORT PINE +#define PE7_WPORT PORTE +#define PE7_DDR DDRE +#define PE7_PWM nullptr + +#undef PF0 +#define PF0_PIN PINF0 +#define PF0_RPORT PINF +#define PF0_WPORT PORTF +#define PF0_DDR DDRF +#define PF0_PWM nullptr +#undef PF1 +#define PF1_PIN PINF1 +#define PF1_RPORT PINF +#define PF1_WPORT PORTF +#define PF1_DDR DDRF +#define PF1_PWM nullptr +#undef PF2 +#define PF2_PIN PINF2 +#define PF2_RPORT PINF +#define PF2_WPORT PORTF +#define PF2_DDR DDRF +#define PF2_PWM nullptr +#undef PF3 +#define PF3_PIN PINF3 +#define PF3_RPORT PINF +#define PF3_WPORT PORTF +#define PF3_DDR DDRF +#define PF3_PWM nullptr +#undef PF4 +#define PF4_PIN PINF4 +#define PF4_RPORT PINF +#define PF4_WPORT PORTF +#define PF4_DDR DDRF +#define PF4_PWM nullptr +#undef PF5 +#define PF5_PIN PINF5 +#define PF5_RPORT PINF +#define PF5_WPORT PORTF +#define PF5_DDR DDRF +#define PF5_PWM nullptr +#undef PF6 +#define PF6_PIN PINF6 +#define PF6_RPORT PINF +#define PF6_WPORT PORTF +#define PF6_DDR DDRF +#define PF6_PWM nullptr +#undef PF7 +#define PF7_PIN PINF7 +#define PF7_RPORT PINF +#define PF7_WPORT PORTF +#define PF7_DDR DDRF +#define PF7_PWM nullptr + +#undef PG0 +#define PG0_PIN PING0 +#define PG0_RPORT PING +#define PG0_WPORT PORTG +#define PG0_DDR DDRG +#define PG0_PWM nullptr +#undef PG1 +#define PG1_PIN PING1 +#define PG1_RPORT PING +#define PG1_WPORT PORTG +#define PG1_DDR DDRG +#define PG1_PWM nullptr +#undef PG2 +#define PG2_PIN PING2 +#define PG2_RPORT PING +#define PG2_WPORT PORTG +#define PG2_DDR DDRG +#define PG2_PWM nullptr +#undef PG3 +#define PG3_PIN PING3 +#define PG3_RPORT PING +#define PG3_WPORT PORTG +#define PG3_DDR DDRG +#define PG3_PWM nullptr +#undef PG4 +#define PG4_PIN PING4 +#define PG4_RPORT PING +#define PG4_WPORT PORTG +#define PG4_DDR DDRG +#define PG4_PWM nullptr +#undef PG5 +#define PG5_PIN PING5 +#define PG5_RPORT PING +#define PG5_WPORT PORTG +#define PG5_DDR DDRG +#define PG5_PWM &OCR0B + +#undef PH0 +#define PH0_PIN PINH0 +#define PH0_RPORT PINH +#define PH0_WPORT PORTH +#define PH0_DDR DDRH +#define PH0_PWM nullptr +#undef PH1 +#define PH1_PIN PINH1 +#define PH1_RPORT PINH +#define PH1_WPORT PORTH +#define PH1_DDR DDRH +#define PH1_PWM nullptr +#undef PH2 +#define PH2_PIN PINH2 +#define PH2_RPORT PINH +#define PH2_WPORT PORTH +#define PH2_DDR DDRH +#define PH2_PWM nullptr +#undef PH3 +#define PH3_PIN PINH3 +#define PH3_RPORT PINH +#define PH3_WPORT PORTH +#define PH3_DDR DDRH +#define PH3_PWM &OCR4AL +#undef PH4 +#define PH4_PIN PINH4 +#define PH4_RPORT PINH +#define PH4_WPORT PORTH +#define PH4_DDR DDRH +#define PH4_PWM &OCR4BL +#undef PH5 +#define PH5_PIN PINH5 +#define PH5_RPORT PINH +#define PH5_WPORT PORTH +#define PH5_DDR DDRH +#define PH5_PWM &OCR4CL +#undef PH6 +#define PH6_PIN PINH6 +#define PH6_RPORT PINH +#define PH6_WPORT PORTH +#define PH6_DDR DDRH +#define PH6_PWM &OCR2B +#undef PH7 +#define PH7_PIN PINH7 +#define PH7_RPORT PINH +#define PH7_WPORT PORTH +#define PH7_DDR DDRH +#define PH7_PWM nullptr + +#undef PJ0 +#define PJ0_PIN PINJ0 +#define PJ0_RPORT PINJ +#define PJ0_WPORT PORTJ +#define PJ0_DDR DDRJ +#define PJ0_PWM nullptr +#undef PJ1 +#define PJ1_PIN PINJ1 +#define PJ1_RPORT PINJ +#define PJ1_WPORT PORTJ +#define PJ1_DDR DDRJ +#define PJ1_PWM nullptr +#undef PJ2 +#define PJ2_PIN PINJ2 +#define PJ2_RPORT PINJ +#define PJ2_WPORT PORTJ +#define PJ2_DDR DDRJ +#define PJ2_PWM nullptr +#undef PJ3 +#define PJ3_PIN PINJ3 +#define PJ3_RPORT PINJ +#define PJ3_WPORT PORTJ +#define PJ3_DDR DDRJ +#define PJ3_PWM nullptr +#undef PJ4 +#define PJ4_PIN PINJ4 +#define PJ4_RPORT PINJ +#define PJ4_WPORT PORTJ +#define PJ4_DDR DDRJ +#define PJ4_PWM nullptr +#undef PJ5 +#define PJ5_PIN PINJ5 +#define PJ5_RPORT PINJ +#define PJ5_WPORT PORTJ +#define PJ5_DDR DDRJ +#define PJ5_PWM nullptr +#undef PJ6 +#define PJ6_PIN PINJ6 +#define PJ6_RPORT PINJ +#define PJ6_WPORT PORTJ +#define PJ6_DDR DDRJ +#define PJ6_PWM nullptr +#undef PJ7 +#define PJ7_PIN PINJ7 +#define PJ7_RPORT PINJ +#define PJ7_WPORT PORTJ +#define PJ7_DDR DDRJ +#define PJ7_PWM nullptr + +#undef PK0 +#define PK0_PIN PINK0 +#define PK0_RPORT PINK +#define PK0_WPORT PORTK +#define PK0_DDR DDRK +#define PK0_PWM nullptr +#undef PK1 +#define PK1_PIN PINK1 +#define PK1_RPORT PINK +#define PK1_WPORT PORTK +#define PK1_DDR DDRK +#define PK1_PWM nullptr +#undef PK2 +#define PK2_PIN PINK2 +#define PK2_RPORT PINK +#define PK2_WPORT PORTK +#define PK2_DDR DDRK +#define PK2_PWM nullptr +#undef PK3 +#define PK3_PIN PINK3 +#define PK3_RPORT PINK +#define PK3_WPORT PORTK +#define PK3_DDR DDRK +#define PK3_PWM nullptr +#undef PK4 +#define PK4_PIN PINK4 +#define PK4_RPORT PINK +#define PK4_WPORT PORTK +#define PK4_DDR DDRK +#define PK4_PWM nullptr +#undef PK5 +#define PK5_PIN PINK5 +#define PK5_RPORT PINK +#define PK5_WPORT PORTK +#define PK5_DDR DDRK +#define PK5_PWM nullptr +#undef PK6 +#define PK6_PIN PINK6 +#define PK6_RPORT PINK +#define PK6_WPORT PORTK +#define PK6_DDR DDRK +#define PK6_PWM nullptr +#undef PK7 +#define PK7_PIN PINK7 +#define PK7_RPORT PINK +#define PK7_WPORT PORTK +#define PK7_DDR DDRK +#define PK7_PWM nullptr + +#undef PL0 +#define PL0_PIN PINL0 +#define PL0_RPORT PINL +#define PL0_WPORT PORTL +#define PL0_DDR DDRL +#define PL0_PWM nullptr +#undef PL1 +#define PL1_PIN PINL1 +#define PL1_RPORT PINL +#define PL1_WPORT PORTL +#define PL1_DDR DDRL +#define PL1_PWM nullptr +#undef PL2 +#define PL2_PIN PINL2 +#define PL2_RPORT PINL +#define PL2_WPORT PORTL +#define PL2_DDR DDRL +#define PL2_PWM nullptr +#undef PL3 +#define PL3_PIN PINL3 +#define PL3_RPORT PINL +#define PL3_WPORT PORTL +#define PL3_DDR DDRL +#define PL3_PWM &OCR5AL +#undef PL4 +#define PL4_PIN PINL4 +#define PL4_RPORT PINL +#define PL4_WPORT PORTL +#define PL4_DDR DDRL +#define PL4_PWM &OCR5BL +#undef PL5 +#define PL5_PIN PINL5 +#define PL5_RPORT PINL +#define PL5_WPORT PORTL +#define PL5_DDR DDRL +#define PL5_PWM &OCR5CL +#undef PL6 +#define PL6_PIN PINL6 +#define PL6_RPORT PINL +#define PL6_WPORT PORTL +#define PL6_DDR DDRL +#define PL6_PWM nullptr +#undef PL7 +#define PL7_PIN PINL7 +#define PL7_RPORT PINL +#define PL7_WPORT PORTL +#define PL7_DDR DDRL +#define PL7_PWM nullptr diff --git a/src/HAL/AVR/fastio/fastio_1281.h b/src/HAL/AVR/fastio/fastio_1281.h new file mode 100644 index 0000000..e0bc5e2 --- /dev/null +++ b/src/HAL/AVR/fastio/fastio_1281.h @@ -0,0 +1,715 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Pin mapping for the 1281 and 2561 + * + * Logical Pin: 38 39 40 41 42 43 44 45 16 10 11 12 06 07 08 09 30 31 32 33 34 35 36 37 17 18 19 20 21 22 23 24 00 01 13 05 02 03 14 15 46 47 48 49 50 51 52 53 25 26 27 28 29 04 + * Port: A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7 G0 G1 G2 G3 G4 G5 + */ + +#include "../fastio.h" + +// change for your board +#define DEBUG_LED DIO46 + +// UART +#define RXD DIO0 +#define TXD DIO1 + +// SPI +#define SCK DIO10 +#define MISO DIO12 +#define MOSI DIO11 +#define SS DIO16 + +// TWI (I2C) +#define SCL DIO17 +#define SDA DIO18 + +// Timers and PWM +#define OC0A DIO9 +#define OC0B DIO4 +#define OC1A DIO7 +#define OC1B DIO8 +#define OC2A DIO6 +#define OC3A DIO5 +#define OC3B DIO2 +#define OC3C DIO3 + +// Digital I/O + +#define DIO0_PIN PINE0 +#define DIO0_RPORT PINE +#define DIO0_WPORT PORTE +#define DIO0_DDR DDRE +#define DIO0_PWM nullptr + +#define DIO1_PIN PINE1 +#define DIO1_RPORT PINE +#define DIO1_WPORT PORTE +#define DIO1_DDR DDRE +#define DIO1_PWM nullptr + +#define DIO2_PIN PINE4 +#define DIO2_RPORT PINE +#define DIO2_WPORT PORTE +#define DIO2_DDR DDRE +#define DIO2_PWM &OCR3BL + +#define DIO3_PIN PINE5 +#define DIO3_RPORT PINE +#define DIO3_WPORT PORTE +#define DIO3_DDR DDRE +#define DIO3_PWM &OCR3CL + +#define DIO4_PIN PING5 +#define DIO4_RPORT PING +#define DIO4_WPORT PORTG +#define DIO4_DDR DDRG +#define DIO4_PWM &OCR0B + +#define DIO5_PIN PINE3 +#define DIO5_RPORT PINE +#define DIO5_WPORT PORTE +#define DIO5_DDR DDRE +#define DIO5_PWM &OCR3AL + +#define DIO6_PIN PINB4 +#define DIO6_RPORT PINB +#define DIO6_WPORT PORTB +#define DIO6_DDR DDRB +#define DIO6_PWM &OCR2AL + +#define DIO7_PIN PINB5 +#define DIO7_RPORT PINB +#define DIO7_WPORT PORTB +#define DIO7_DDR DDRB +#define DIO7_PWM &OCR1AL + +#define DIO8_PIN PINB6 +#define DIO8_RPORT PINB +#define DIO8_WPORT PORTB +#define DIO8_DDR DDRB +#define DIO8_PWM &OCR1BL + +#define DIO9_PIN PINB7 +#define DIO9_RPORT PINB +#define DIO9_WPORT PORTB +#define DIO9_DDR DDRB +#define DIO9_PWM &OCR0AL + +#define DIO10_PIN PINB1 +#define DIO10_RPORT PINB +#define DIO10_WPORT PORTB +#define DIO10_DDR DDRB +#define DIO10_PWM nullptr + +#define DIO11_PIN PINB2 +#define DIO11_RPORT PINB +#define DIO11_WPORT PORTB +#define DIO11_DDR DDRB +#define DIO11_PWM nullptr + +#define DIO12_PIN PINB3 +#define DIO12_RPORT PINB +#define DIO12_WPORT PORTB +#define DIO12_DDR DDRB +#define DIO12_PWM nullptr + +#define DIO13_PIN PINE2 +#define DIO13_RPORT PINE +#define DIO13_WPORT PORTE +#define DIO13_DDR DDRE +#define DIO13_PWM nullptr + +#define DIO14_PIN PINE6 +#define DIO14_RPORT PINE +#define DIO14_WPORT PORTE +#define DIO14_DDR DDRE +#define DIO14_PWM nullptr + +#define DIO15_PIN PINE7 +#define DIO15_RPORT PINE +#define DIO15_WPORT PORTE +#define DIO15_DDR DDRE +#define DIO15_PWM nullptr + +#define DIO16_PIN PINB0 +#define DIO16_RPORT PINB +#define DIO16_WPORT PORTB +#define DIO16_DDR DDRB +#define DIO16_PWM nullptr + +#define DIO17_PIN PIND0 +#define DIO17_RPORT PIND +#define DIO17_WPORT PORTD +#define DIO17_DDR DDRD +#define DIO17_PWM nullptr + +#define DIO18_PIN PIND1 +#define DIO18_RPORT PIND +#define DIO18_WPORT PORTD +#define DIO18_DDR DDRD +#define DIO18_PWM nullptr + +#define DIO19_PIN PIND2 +#define DIO19_RPORT PIND +#define DIO19_WPORT PORTD +#define DIO19_DDR DDRD +#define DIO19_PWM nullptr + +#define DIO20_PIN PIND3 +#define DIO20_RPORT PIND +#define DIO20_WPORT PORTD +#define DIO20_DDR DDRD +#define DIO20_PWM nullptr + +#define DIO21_PIN PIND4 +#define DIO21_RPORT PIND +#define DIO21_WPORT PORTD +#define DIO21_DDR DDRD +#define DIO21_PWM nullptr + +#define DIO22_PIN PIND5 +#define DIO22_RPORT PIND +#define DIO22_WPORT PORTD +#define DIO22_DDR DDRD +#define DIO22_PWM nullptr + +#define DIO23_PIN PIND6 +#define DIO23_RPORT PIND +#define DIO23_WPORT PORTD +#define DIO23_DDR DDRD +#define DIO23_PWM nullptr + +#define DIO24_PIN PIND7 +#define DIO24_RPORT PIND +#define DIO24_WPORT PORTD +#define DIO24_DDR DDRD +#define DIO24_PWM nullptr + +#define DIO25_PIN PING0 +#define DIO25_RPORT PING +#define DIO25_WPORT PORTG +#define DIO25_DDR DDRG +#define DIO25_PWM nullptr + +#define DIO26_PIN PING1 +#define DIO26_RPORT PING +#define DIO26_WPORT PORTG +#define DIO26_DDR DDRG +#define DIO26_PWM nullptr + +#define DIO27_PIN PING2 +#define DIO27_RPORT PING +#define DIO27_WPORT PORTG +#define DIO27_DDR DDRG +#define DIO27_PWM nullptr + +#define DIO28_PIN PING3 +#define DIO28_RPORT PING +#define DIO28_WPORT PORTG +#define DIO28_DDR DDRG +#define DIO28_PWM nullptr + +#define DIO29_PIN PING4 +#define DIO29_RPORT PING +#define DIO29_WPORT PORTG +#define DIO29_DDR DDRG +#define DIO29_PWM nullptr + +#define DIO30_PIN PINC0 +#define DIO30_RPORT PINC +#define DIO30_WPORT PORTC +#define DIO30_DDR DDRC +#define DIO30_PWM nullptr + +#define DIO31_PIN PINC1 +#define DIO31_RPORT PINC +#define DIO31_WPORT PORTC +#define DIO31_DDR DDRC +#define DIO31_PWM nullptr + +#define DIO32_PIN PINC2 +#define DIO32_RPORT PINC +#define DIO32_WPORT PORTC +#define DIO32_DDR DDRC +#define DIO32_PWM nullptr + +#define DIO33_PIN PINC3 +#define DIO33_RPORT PINC +#define DIO33_WPORT PORTC +#define DIO33_DDR DDRC +#define DIO33_PWM nullptr + +#define DIO34_PIN PINC4 +#define DIO34_RPORT PINC +#define DIO34_WPORT PORTC +#define DIO34_DDR DDRC +#define DIO34_PWM nullptr + +#define DIO35_PIN PINC5 +#define DIO35_RPORT PINC +#define DIO35_WPORT PORTC +#define DIO35_DDR DDRC +#define DIO35_PWM nullptr + +#define DIO36_PIN PINC6 +#define DIO36_RPORT PINC +#define DIO36_WPORT PORTC +#define DIO36_DDR DDRC +#define DIO36_PWM nullptr + +#define DIO37_PIN PINC7 +#define DIO37_RPORT PINC +#define DIO37_WPORT PORTC +#define DIO37_DDR DDRC +#define DIO37_PWM nullptr + +#define DIO38_PIN PINA0 +#define DIO38_RPORT PINA +#define DIO38_WPORT PORTA +#define DIO38_DDR DDRA +#define DIO38_PWM nullptr + +#define DIO39_PIN PINA1 +#define DIO39_RPORT PINA +#define DIO39_WPORT PORTA +#define DIO39_DDR DDRA +#define DIO39_PWM nullptr + +#define DIO40_PIN PINA2 +#define DIO40_RPORT PINA +#define DIO40_WPORT PORTA +#define DIO40_DDR DDRA +#define DIO40_PWM nullptr + +#define DIO41_PIN PINA3 +#define DIO41_RPORT PINA +#define DIO41_WPORT PORTA +#define DIO41_DDR DDRA +#define DIO41_PWM nullptr + +#define DIO42_PIN PINA4 +#define DIO42_RPORT PINA +#define DIO42_WPORT PORTA +#define DIO42_DDR DDRA +#define DIO42_PWM nullptr + +#define DIO43_PIN PINA5 +#define DIO43_RPORT PINA +#define DIO43_WPORT PORTA +#define DIO43_DDR DDRA +#define DIO43_PWM nullptr + +#define DIO44_PIN PINA6 +#define DIO44_RPORT PINA +#define DIO44_WPORT PORTA +#define DIO44_DDR DDRA +#define DIO44_PWM nullptr + +#define DIO45_PIN PINA7 +#define DIO45_RPORT PINA +#define DIO45_WPORT PORTA +#define DIO45_DDR DDRA +#define DIO45_PWM nullptr + +#define DIO46_PIN PINF0 +#define DIO46_RPORT PINF +#define DIO46_WPORT PORTF +#define DIO46_DDR DDRF +#define DIO46_PWM nullptr + +#define DIO47_PIN PINF1 +#define DIO47_RPORT PINF +#define DIO47_WPORT PORTF +#define DIO47_DDR DDRF +#define DIO47_PWM nullptr + +#define DIO48_PIN PINF2 +#define DIO48_RPORT PINF +#define DIO48_WPORT PORTF +#define DIO48_DDR DDRF +#define DIO48_PWM nullptr + +#define DIO49_PIN PINF3 +#define DIO49_RPORT PINF +#define DIO49_WPORT PORTF +#define DIO49_DDR DDRF +#define DIO49_PWM nullptr + +#define DIO50_PIN PINF4 +#define DIO50_RPORT PINF +#define DIO50_WPORT PORTF +#define DIO50_DDR DDRF +#define DIO50_PWM nullptr + +#define DIO51_PIN PINF5 +#define DIO51_RPORT PINF +#define DIO51_WPORT PORTF +#define DIO51_DDR DDRF +#define DIO51_PWM nullptr + +#define DIO52_PIN PINF6 +#define DIO52_RPORT PINF +#define DIO52_WPORT PORTF +#define DIO52_DDR DDRF +#define DIO52_PWM nullptr + +#define DIO53_PIN PINF7 +#define DIO53_RPORT PINF +#define DIO53_WPORT PORTF +#define DIO53_DDR DDRF +#define DIO53_PWM nullptr + +#undef PA0 +#define PA0_PIN PINA0 +#define PA0_RPORT PINA +#define PA0_WPORT PORTA +#define PA0_DDR DDRA +#define PA0_PWM nullptr +#undef PA1 +#define PA1_PIN PINA1 +#define PA1_RPORT PINA +#define PA1_WPORT PORTA +#define PA1_DDR DDRA +#define PA1_PWM nullptr +#undef PA2 +#define PA2_PIN PINA2 +#define PA2_RPORT PINA +#define PA2_WPORT PORTA +#define PA2_DDR DDRA +#define PA2_PWM nullptr +#undef PA3 +#define PA3_PIN PINA3 +#define PA3_RPORT PINA +#define PA3_WPORT PORTA +#define PA3_DDR DDRA +#define PA3_PWM nullptr +#undef PA4 +#define PA4_PIN PINA4 +#define PA4_RPORT PINA +#define PA4_WPORT PORTA +#define PA4_DDR DDRA +#define PA4_PWM nullptr +#undef PA5 +#define PA5_PIN PINA5 +#define PA5_RPORT PINA +#define PA5_WPORT PORTA +#define PA5_DDR DDRA +#define PA5_PWM nullptr +#undef PA6 +#define PA6_PIN PINA6 +#define PA6_RPORT PINA +#define PA6_WPORT PORTA +#define PA6_DDR DDRA +#define PA6_PWM nullptr +#undef PA7 +#define PA7_PIN PINA7 +#define PA7_RPORT PINA +#define PA7_WPORT PORTA +#define PA7_DDR DDRA +#define PA7_PWM nullptr + +#undef PB0 +#define PB0_PIN PINB0 +#define PB0_RPORT PINB +#define PB0_WPORT PORTB +#define PB0_DDR DDRB +#define PB0_PWM nullptr +#undef PB1 +#define PB1_PIN PINB1 +#define PB1_RPORT PINB +#define PB1_WPORT PORTB +#define PB1_DDR DDRB +#define PB1_PWM nullptr +#undef PB2 +#define PB2_PIN PINB2 +#define PB2_RPORT PINB +#define PB2_WPORT PORTB +#define PB2_DDR DDRB +#define PB2_PWM nullptr +#undef PB3 +#define PB3_PIN PINB3 +#define PB3_RPORT PINB +#define PB3_WPORT PORTB +#define PB3_DDR DDRB +#define PB3_PWM nullptr +#undef PB4 +#define PB4_PIN PINB4 +#define PB4_RPORT PINB +#define PB4_WPORT PORTB +#define PB4_DDR DDRB +#define PB4_PWM &OCR2A +#undef PB5 +#define PB5_PIN PINB5 +#define PB5_RPORT PINB +#define PB5_WPORT PORTB +#define PB5_DDR DDRB +#define PB5_PWM nullptr +#undef PB6 +#define PB6_PIN PINB6 +#define PB6_RPORT PINB +#define PB6_WPORT PORTB +#define PB6_DDR DDRB +#define PB6_PWM nullptr +#undef PB7 +#define PB7_PIN PINB7 +#define PB7_RPORT PINB +#define PB7_WPORT PORTB +#define PB7_DDR DDRB +#define PB7_PWM &OCR0A + +#undef PC0 +#define PC0_PIN PINC0 +#define PC0_RPORT PINC +#define PC0_WPORT PORTC +#define PC0_DDR DDRC +#define PC0_PWM nullptr +#undef PC1 +#define PC1_PIN PINC1 +#define PC1_RPORT PINC +#define PC1_WPORT PORTC +#define PC1_DDR DDRC +#define PC1_PWM nullptr +#undef PC2 +#define PC2_PIN PINC2 +#define PC2_RPORT PINC +#define PC2_WPORT PORTC +#define PC2_DDR DDRC +#define PC2_PWM nullptr +#undef PC3 +#define PC3_PIN PINC3 +#define PC3_RPORT PINC +#define PC3_WPORT PORTC +#define PC3_DDR DDRC +#define PC3_PWM nullptr +#undef PC4 +#define PC4_PIN PINC4 +#define PC4_RPORT PINC +#define PC4_WPORT PORTC +#define PC4_DDR DDRC +#define PC4_PWM nullptr +#undef PC5 +#define PC5_PIN PINC5 +#define PC5_RPORT PINC +#define PC5_WPORT PORTC +#define PC5_DDR DDRC +#define PC5_PWM nullptr +#undef PC6 +#define PC6_PIN PINC6 +#define PC6_RPORT PINC +#define PC6_WPORT PORTC +#define PC6_DDR DDRC +#define PC6_PWM nullptr +#undef PC7 +#define PC7_PIN PINC7 +#define PC7_RPORT PINC +#define PC7_WPORT PORTC +#define PC7_DDR DDRC +#define PC7_PWM nullptr + +#undef PD0 +#define PD0_PIN PIND0 +#define PD0_RPORT PIND +#define PD0_WPORT PORTD +#define PD0_DDR DDRD +#define PD0_PWM nullptr +#undef PD1 +#define PD1_PIN PIND1 +#define PD1_RPORT PIND +#define PD1_WPORT PORTD +#define PD1_DDR DDRD +#define PD1_PWM nullptr +#undef PD2 +#define PD2_PIN PIND2 +#define PD2_RPORT PIND +#define PD2_WPORT PORTD +#define PD2_DDR DDRD +#define PD2_PWM nullptr +#undef PD3 +#define PD3_PIN PIND3 +#define PD3_RPORT PIND +#define PD3_WPORT PORTD +#define PD3_DDR DDRD +#define PD3_PWM nullptr +#undef PD4 +#define PD4_PIN PIND4 +#define PD4_RPORT PIND +#define PD4_WPORT PORTD +#define PD4_DDR DDRD +#define PD4_PWM nullptr +#undef PD5 +#define PD5_PIN PIND5 +#define PD5_RPORT PIND +#define PD5_WPORT PORTD +#define PD5_DDR DDRD +#define PD5_PWM nullptr +#undef PD6 +#define PD6_PIN PIND6 +#define PD6_RPORT PIND +#define PD6_WPORT PORTD +#define PD6_DDR DDRD +#define PD6_PWM nullptr +#undef PD7 +#define PD7_PIN PIND7 +#define PD7_RPORT PIND +#define PD7_WPORT PORTD +#define PD7_DDR DDRD +#define PD7_PWM nullptr + +#undef PE0 +#define PE0_PIN PINE0 +#define PE0_RPORT PINE +#define PE0_WPORT PORTE +#define PE0_DDR DDRE +#define PE0_PWM nullptr +#undef PE1 +#define PE1_PIN PINE1 +#define PE1_RPORT PINE +#define PE1_WPORT PORTE +#define PE1_DDR DDRE +#define PE1_PWM nullptr +#undef PE2 +#define PE2_PIN PINE2 +#define PE2_RPORT PINE +#define PE2_WPORT PORTE +#define PE2_DDR DDRE +#define PE2_PWM nullptr +#undef PE3 +#define PE3_PIN PINE3 +#define PE3_RPORT PINE +#define PE3_WPORT PORTE +#define PE3_DDR DDRE +#define PE3_PWM &OCR3AL +#undef PE4 +#define PE4_PIN PINE4 +#define PE4_RPORT PINE +#define PE4_WPORT PORTE +#define PE4_DDR DDRE +#define PE4_PWM &OCR3BL +#undef PE5 +#define PE5_PIN PINE5 +#define PE5_RPORT PINE +#define PE5_WPORT PORTE +#define PE5_DDR DDRE +#define PE5_PWM &OCR3CL +#undef PE6 +#define PE6_PIN PINE6 +#define PE6_RPORT PINE +#define PE6_WPORT PORTE +#define PE6_DDR DDRE +#define PE6_PWM nullptr +#undef PE7 +#define PE7_PIN PINE7 +#define PE7_RPORT PINE +#define PE7_WPORT PORTE +#define PE7_DDR DDRE +#define PE7_PWM nullptr + +#undef PF0 +#define PF0_PIN PINF0 +#define PF0_RPORT PINF +#define PF0_WPORT PORTF +#define PF0_DDR DDRF +#define PF0_PWM nullptr +#undef PF1 +#define PF1_PIN PINF1 +#define PF1_RPORT PINF +#define PF1_WPORT PORTF +#define PF1_DDR DDRF +#define PF1_PWM nullptr +#undef PF2 +#define PF2_PIN PINF2 +#define PF2_RPORT PINF +#define PF2_WPORT PORTF +#define PF2_DDR DDRF +#define PF2_PWM nullptr +#undef PF3 +#define PF3_PIN PINF3 +#define PF3_RPORT PINF +#define PF3_WPORT PORTF +#define PF3_DDR DDRF +#define PF3_PWM nullptr +#undef PF4 +#define PF4_PIN PINF4 +#define PF4_RPORT PINF +#define PF4_WPORT PORTF +#define PF4_DDR DDRF +#define PF4_PWM nullptr +#undef PF5 +#define PF5_PIN PINF5 +#define PF5_RPORT PINF +#define PF5_WPORT PORTF +#define PF5_DDR DDRF +#define PF5_PWM nullptr +#undef PF6 +#define PF6_PIN PINF6 +#define PF6_RPORT PINF +#define PF6_WPORT PORTF +#define PF6_DDR DDRF +#define PF6_PWM nullptr +#undef PF7 +#define PF7_PIN PINF7 +#define PF7_RPORT PINF +#define PF7_WPORT PORTF +#define PF7_DDR DDRF +#define PF7_PWM nullptr + +#undef PG0 +#define PG0_PIN PING0 +#define PG0_RPORT PING +#define PG0_WPORT PORTG +#define PG0_DDR DDRG +#define PG0_PWM nullptr +#undef PG1 +#define PG1_PIN PING1 +#define PG1_RPORT PING +#define PG1_WPORT PORTG +#define PG1_DDR DDRG +#define PG1_PWM nullptr +#undef PG2 +#define PG2_PIN PING2 +#define PG2_RPORT PING +#define PG2_WPORT PORTG +#define PG2_DDR DDRG +#define PG2_PWM nullptr +#undef PG3 +#define PG3_PIN PING3 +#define PG3_RPORT PING +#define PG3_WPORT PORTG +#define PG3_DDR DDRG +#define PG3_PWM nullptr +#undef PG4 +#define PG4_PIN PING4 +#define PG4_RPORT PING +#define PG4_WPORT PORTG +#define PG4_DDR DDRG +#define PG4_PWM nullptr +#undef PG5 +#define PG5_PIN PING5 +#define PG5_RPORT PING +#define PG5_WPORT PORTG +#define PG5_DDR DDRG +#define PG5_PWM &OCR0B diff --git a/src/HAL/AVR/fastio/fastio_168.h b/src/HAL/AVR/fastio/fastio_168.h new file mode 100644 index 0000000..8cfdd1e --- /dev/null +++ b/src/HAL/AVR/fastio/fastio_168.h @@ -0,0 +1,357 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Pin mapping for the 168, 328, and 328P + * + * Logical Pin: 08 09 10 11 12 13 14 15 16 17 18 19 20 21 00 01 02 03 04 05 06 07 + * Port: B0 B1 B2 B3 B4 B5 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 + */ + +#include "../fastio.h" + +#define DEBUG_LED AIO5 + +// UART +#define RXD DIO0 +#define TXD DIO1 + +// SPI +#define SCK DIO13 +#define MISO DIO12 +#define MOSI DIO11 +#define SS DIO10 + +// TWI (I2C) +#define SCL AIO5 +#define SDA AIO4 + +// Timers and PWM +#define OC0A DIO6 +#define OC0B DIO5 +#define OC1A DIO9 +#define OC1B DIO10 +#define OC2A DIO11 +#define OC2B DIO3 + +// Digital I/O + +#define DIO0_PIN PIND0 +#define DIO0_RPORT PIND +#define DIO0_WPORT PORTD +#define DIO0_DDR DDRD +#define DIO0_PWM nullptr + +#define DIO1_PIN PIND1 +#define DIO1_RPORT PIND +#define DIO1_WPORT PORTD +#define DIO1_DDR DDRD +#define DIO1_PWM nullptr + +#define DIO2_PIN PIND2 +#define DIO2_RPORT PIND +#define DIO2_WPORT PORTD +#define DIO2_DDR DDRD +#define DIO2_PWM nullptr + +#define DIO3_PIN PIND3 +#define DIO3_RPORT PIND +#define DIO3_WPORT PORTD +#define DIO3_DDR DDRD +#define DIO3_PWM &OCR2B + +#define DIO4_PIN PIND4 +#define DIO4_RPORT PIND +#define DIO4_WPORT PORTD +#define DIO4_DDR DDRD +#define DIO4_PWM nullptr + +#define DIO5_PIN PIND5 +#define DIO5_RPORT PIND +#define DIO5_WPORT PORTD +#define DIO5_DDR DDRD +#define DIO5_PWM &OCR0B + +#define DIO6_PIN PIND6 +#define DIO6_RPORT PIND +#define DIO6_WPORT PORTD +#define DIO6_DDR DDRD +#define DIO6_PWM &OCR0A + +#define DIO7_PIN PIND7 +#define DIO7_RPORT PIND +#define DIO7_WPORT PORTD +#define DIO7_DDR DDRD +#define DIO7_PWM nullptr + +#define DIO8_PIN PINB0 +#define DIO8_RPORT PINB +#define DIO8_WPORT PORTB +#define DIO8_DDR DDRB +#define DIO8_PWM nullptr + +#define DIO9_PIN PINB1 +#define DIO9_RPORT PINB +#define DIO9_WPORT PORTB +#define DIO9_DDR DDRB +#define DIO9_PWM nullptr + +#define DIO10_PIN PINB2 +#define DIO10_RPORT PINB +#define DIO10_WPORT PORTB +#define DIO10_DDR DDRB +#define DIO10_PWM nullptr + +#define DIO11_PIN PINB3 +#define DIO11_RPORT PINB +#define DIO11_WPORT PORTB +#define DIO11_DDR DDRB +#define DIO11_PWM &OCR2A + +#define DIO12_PIN PINB4 +#define DIO12_RPORT PINB +#define DIO12_WPORT PORTB +#define DIO12_DDR DDRB +#define DIO12_PWM nullptr + +#define DIO13_PIN PINB5 +#define DIO13_RPORT PINB +#define DIO13_WPORT PORTB +#define DIO13_DDR DDRB +#define DIO13_PWM nullptr + +#define DIO14_PIN PINC0 +#define DIO14_RPORT PINC +#define DIO14_WPORT PORTC +#define DIO14_DDR DDRC +#define DIO14_PWM nullptr + +#define DIO15_PIN PINC1 +#define DIO15_RPORT PINC +#define DIO15_WPORT PORTC +#define DIO15_DDR DDRC +#define DIO15_PWM nullptr + +#define DIO16_PIN PINC2 +#define DIO16_RPORT PINC +#define DIO16_WPORT PORTC +#define DIO16_DDR DDRC +#define DIO16_PWM nullptr + +#define DIO17_PIN PINC3 +#define DIO17_RPORT PINC +#define DIO17_WPORT PORTC +#define DIO17_DDR DDRC +#define DIO17_PWM nullptr + +#define DIO18_PIN PINC4 +#define DIO18_RPORT PINC +#define DIO18_WPORT PORTC +#define DIO18_DDR DDRC +#define DIO18_PWM nullptr + +#define DIO19_PIN PINC5 +#define DIO19_RPORT PINC +#define DIO19_WPORT PORTC +#define DIO19_DDR DDRC +#define DIO19_PWM nullptr + +#define DIO20_PIN PINC6 +#define DIO20_RPORT PINC +#define DIO20_WPORT PORTC +#define DIO20_DDR DDRC +#define DIO20_PWM nullptr + +#define DIO21_PIN PINC7 +#define DIO21_RPORT PINC +#define DIO21_WPORT PORTC +#define DIO21_DDR DDRC +#define DIO21_PWM nullptr + +#undef PB0 +#define PB0_PIN PINB0 +#define PB0_RPORT PINB +#define PB0_WPORT PORTB +#define PB0_DDR DDRB +#define PB0_PWM nullptr + +#undef PB1 +#define PB1_PIN PINB1 +#define PB1_RPORT PINB +#define PB1_WPORT PORTB +#define PB1_DDR DDRB +#define PB1_PWM nullptr + +#undef PB2 +#define PB2_PIN PINB2 +#define PB2_RPORT PINB +#define PB2_WPORT PORTB +#define PB2_DDR DDRB +#define PB2_PWM nullptr + +#undef PB3 +#define PB3_PIN PINB3 +#define PB3_RPORT PINB +#define PB3_WPORT PORTB +#define PB3_DDR DDRB +#define PB3_PWM &OCR2A + +#undef PB4 +#define PB4_PIN PINB4 +#define PB4_RPORT PINB +#define PB4_WPORT PORTB +#define PB4_DDR DDRB +#define PB4_PWM nullptr + +#undef PB5 +#define PB5_PIN PINB5 +#define PB5_RPORT PINB +#define PB5_WPORT PORTB +#define PB5_DDR DDRB +#define PB5_PWM nullptr + +#undef PB6 +#define PB6_PIN PINB6 +#define PB6_RPORT PINB +#define PB6_WPORT PORTB +#define PB6_DDR DDRB +#define PB6_PWM nullptr + +#undef PB7 +#define PB7_PIN PINB7 +#define PB7_RPORT PINB +#define PB7_WPORT PORTB +#define PB7_DDR DDRB +#define PB7_PWM nullptr + +#undef PC0 +#define PC0_PIN PINC0 +#define PC0_RPORT PINC +#define PC0_WPORT PORTC +#define PC0_DDR DDRC +#define PC0_PWM nullptr + +#undef PC1 +#define PC1_PIN PINC1 +#define PC1_RPORT PINC +#define PC1_WPORT PORTC +#define PC1_DDR DDRC +#define PC1_PWM nullptr + +#undef PC2 +#define PC2_PIN PINC2 +#define PC2_RPORT PINC +#define PC2_WPORT PORTC +#define PC2_DDR DDRC +#define PC2_PWM nullptr + +#undef PC3 +#define PC3_PIN PINC3 +#define PC3_RPORT PINC +#define PC3_WPORT PORTC +#define PC3_DDR DDRC +#define PC3_PWM nullptr + +#undef PC4 +#define PC4_PIN PINC4 +#define PC4_RPORT PINC +#define PC4_WPORT PORTC +#define PC4_DDR DDRC +#define PC4_PWM nullptr + +#undef PC5 +#define PC5_PIN PINC5 +#define PC5_RPORT PINC +#define PC5_WPORT PORTC +#define PC5_DDR DDRC +#define PC5_PWM nullptr + +#undef PC6 +#define PC6_PIN PINC6 +#define PC6_RPORT PINC +#define PC6_WPORT PORTC +#define PC6_DDR DDRC +#define PC6_PWM nullptr + +#undef PC7 +#define PC7_PIN PINC7 +#define PC7_RPORT PINC +#define PC7_WPORT PORTC +#define PC7_DDR DDRC +#define PC7_PWM nullptr + +#undef PD0 +#define PD0_PIN PIND0 +#define PD0_RPORT PIND +#define PD0_WPORT PORTD +#define PD0_DDR DDRD +#define PD0_PWM nullptr + +#undef PD1 +#define PD1_PIN PIND1 +#define PD1_RPORT PIND +#define PD1_WPORT PORTD +#define PD1_DDR DDRD +#define PD1_PWM nullptr + +#undef PD2 +#define PD2_PIN PIND2 +#define PD2_RPORT PIND +#define PD2_WPORT PORTD +#define PD2_DDR DDRD +#define PD2_PWM nullptr + +#undef PD3 +#define PD3_PIN PIND3 +#define PD3_RPORT PIND +#define PD3_WPORT PORTD +#define PD3_DDR DDRD +#define PD3_PWM &OCR2B + +#undef PD4 +#define PD4_PIN PIND4 +#define PD4_RPORT PIND +#define PD4_WPORT PORTD +#define PD4_DDR DDRD +#define PD4_PWM nullptr + +#undef PD5 +#define PD5_PIN PIND5 +#define PD5_RPORT PIND +#define PD5_WPORT PORTD +#define PD5_DDR DDRD +#define PD5_PWM &OCR0B + +#undef PD6 +#define PD6_PIN PIND6 +#define PD6_RPORT PIND +#define PD6_WPORT PORTD +#define PD6_DDR DDRD +#define PD6_PWM &OCR0A + +#undef PD7 +#define PD7_PIN PIND7 +#define PD7_RPORT PIND +#define PD7_WPORT PORTD +#define PD7_DDR DDRD +#define PD7_PWM nullptr diff --git a/src/HAL/AVR/fastio/fastio_644.h b/src/HAL/AVR/fastio/fastio_644.h new file mode 100644 index 0000000..f4a9427 --- /dev/null +++ b/src/HAL/AVR/fastio/fastio_644.h @@ -0,0 +1,552 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Pin mapping for the 644, 644p, 644pa, and 1284p + * + * Logical Pin: 00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 + * Port: B0 B1 B2 B3 B4 B5 B6 B7 D0 D1 D2 D3 D4 D5 D6 D7 C0 C1 C2 C3 C4 C5 C6 C7 A7 A6 A5 A4 A3 A2 A1 A0 + */ + +/** ATMega644 + * + * +---\/---+ + * (D 0) PB0 1| |40 PA0 (AI 0 / D31) + * (D 1) PB1 2| |39 PA1 (AI 1 / D30) + * INT2 (D 2) PB2 3| |38 PA2 (AI 2 / D29) + * PWM (D 3) PB3 4| |37 PA3 (AI 3 / D28) + * PWM (D 4) PB4 5| |36 PA4 (AI 4 / D27) + * MOSI (D 5) PB5 6| |35 PA5 (AI 5 / D26) + * MISO (D 6) PB6 7| |34 PA6 (AI 6 / D25) + * SCK (D 7) PB7 8| |33 PA7 (AI 7 / D24) + * RST 9| |32 AREF + * VCC 10| |31 GND + * GND 11| |30 AVCC + * XTAL2 12| |29 PC7 (D 23) + * XTAL1 13| |28 PC6 (D 22) + * RX0 (D 8) PD0 14| |27 PC5 (D 21) TDI + * TX0 (D 9) PD1 15| |26 PC4 (D 20) TDO + * INT0 RX1 (D 10) PD2 16| |25 PC3 (D 19) TMS + * INT1 TX1 (D 11) PD3 17| |24 PC2 (D 18) TCK + * PWM (D 12) PD4 18| |23 PC1 (D 17) SDA + * PWM (D 13) PD5 19| |22 PC0 (D 16) SCL + * PWM (D 14) PD6 20| |21 PD7 (D 15) PWM + * +--------+ + */ + +#include "../fastio.h" + +#define DEBUG_LED DIO0 + +// UART +#define RXD DIO8 +#define TXD DIO9 +#define RXD0 DIO8 +#define TXD0 DIO9 + +#define RXD1 DIO10 +#define TXD1 DIO11 + +// SPI +#define SCK DIO7 +#define MISO DIO6 +#define MOSI DIO5 +#define SS DIO4 + +// TWI (I2C) +#define SCL DIO16 +#define SDA DIO17 + +// Timers and PWM +#define OC0A DIO3 +#define OC0B DIO4 +#define OC1A DIO13 +#define OC1B DIO12 +#define OC2A DIO15 +#define OC2B DIO14 + +// Digital I/O + +#define DIO0_PIN PINB0 +#define DIO0_RPORT PINB +#define DIO0_WPORT PORTB +#define DIO0_DDR DDRB +#define DIO0_PWM nullptr + +#define DIO1_PIN PINB1 +#define DIO1_RPORT PINB +#define DIO1_WPORT PORTB +#define DIO1_DDR DDRB +#define DIO1_PWM nullptr + +#define DIO2_PIN PINB2 +#define DIO2_RPORT PINB +#define DIO2_WPORT PORTB +#define DIO2_DDR DDRB +#define DIO2_PWM nullptr + +#define DIO3_PIN PINB3 +#define DIO3_RPORT PINB +#define DIO3_WPORT PORTB +#define DIO3_DDR DDRB +#define DIO3_PWM &OCR0A + +#define DIO4_PIN PINB4 +#define DIO4_RPORT PINB +#define DIO4_WPORT PORTB +#define DIO4_DDR DDRB +#define DIO4_PWM &OCR0B + +#define DIO5_PIN PINB5 +#define DIO5_RPORT PINB +#define DIO5_WPORT PORTB +#define DIO5_DDR DDRB +#define DIO5_PWM nullptr + +#define DIO6_PIN PINB6 +#define DIO6_RPORT PINB +#define DIO6_WPORT PORTB +#define DIO6_DDR DDRB +#define DIO6_PWM nullptr + +#define DIO7_PIN PINB7 +#define DIO7_RPORT PINB +#define DIO7_WPORT PORTB +#define DIO7_DDR DDRB +#define DIO7_PWM nullptr + +#define DIO8_PIN PIND0 +#define DIO8_RPORT PIND +#define DIO8_WPORT PORTD +#define DIO8_DDR DDRD +#define DIO8_PWM nullptr + +#define DIO9_PIN PIND1 +#define DIO9_RPORT PIND +#define DIO9_WPORT PORTD +#define DIO9_DDR DDRD +#define DIO9_PWM nullptr + +#define DIO10_PIN PIND2 +#define DIO10_RPORT PIND +#define DIO10_WPORT PORTD +#define DIO10_DDR DDRD +#define DIO10_PWM nullptr + +#define DIO11_PIN PIND3 +#define DIO11_RPORT PIND +#define DIO11_WPORT PORTD +#define DIO11_DDR DDRD +#define DIO11_PWM nullptr + +#define DIO12_PIN PIND4 +#define DIO12_RPORT PIND +#define DIO12_WPORT PORTD +#define DIO12_DDR DDRD +#define DIO12_PWM &OCR1B + +#define DIO13_PIN PIND5 +#define DIO13_RPORT PIND +#define DIO13_WPORT PORTD +#define DIO13_DDR DDRD +#define DIO13_PWM &OCR1A + +#define DIO14_PIN PIND6 +#define DIO14_RPORT PIND +#define DIO14_WPORT PORTD +#define DIO14_DDR DDRD +#define DIO14_PWM &OCR2B + +#define DIO15_PIN PIND7 +#define DIO15_RPORT PIND +#define DIO15_WPORT PORTD +#define DIO15_DDR DDRD +#define DIO15_PWM &OCR2A + +#define DIO16_PIN PINC0 +#define DIO16_RPORT PINC +#define DIO16_WPORT PORTC +#define DIO16_DDR DDRC +#define DIO16_PWM nullptr + +#define DIO17_PIN PINC1 +#define DIO17_RPORT PINC +#define DIO17_WPORT PORTC +#define DIO17_DDR DDRC +#define DIO17_PWM nullptr + +#define DIO18_PIN PINC2 +#define DIO18_RPORT PINC +#define DIO18_WPORT PORTC +#define DIO18_DDR DDRC +#define DIO18_PWM nullptr + +#define DIO19_PIN PINC3 +#define DIO19_RPORT PINC +#define DIO19_WPORT PORTC +#define DIO19_DDR DDRC +#define DIO19_PWM nullptr + +#define DIO20_PIN PINC4 +#define DIO20_RPORT PINC +#define DIO20_WPORT PORTC +#define DIO20_DDR DDRC +#define DIO20_PWM nullptr + +#define DIO21_PIN PINC5 +#define DIO21_RPORT PINC +#define DIO21_WPORT PORTC +#define DIO21_DDR DDRC +#define DIO21_PWM nullptr + +#define DIO22_PIN PINC6 +#define DIO22_RPORT PINC +#define DIO22_WPORT PORTC +#define DIO22_DDR DDRC +#define DIO22_PWM nullptr + +#define DIO23_PIN PINC7 +#define DIO23_RPORT PINC +#define DIO23_WPORT PORTC +#define DIO23_DDR DDRC +#define DIO23_PWM nullptr + +#define DIO24_PIN PINA7 +#define DIO24_RPORT PINA +#define DIO24_WPORT PORTA +#define DIO24_DDR DDRA +#define DIO24_PWM nullptr + +#define DIO25_PIN PINA6 +#define DIO25_RPORT PINA +#define DIO25_WPORT PORTA +#define DIO25_DDR DDRA +#define DIO25_PWM nullptr + +#define DIO26_PIN PINA5 +#define DIO26_RPORT PINA +#define DIO26_WPORT PORTA +#define DIO26_DDR DDRA +#define DIO26_PWM nullptr + +#define DIO27_PIN PINA4 +#define DIO27_RPORT PINA +#define DIO27_WPORT PORTA +#define DIO27_DDR DDRA +#define DIO27_PWM nullptr + +#define DIO28_PIN PINA3 +#define DIO28_RPORT PINA +#define DIO28_WPORT PORTA +#define DIO28_DDR DDRA +#define DIO28_PWM nullptr + +#define DIO29_PIN PINA2 +#define DIO29_RPORT PINA +#define DIO29_WPORT PORTA +#define DIO29_DDR DDRA +#define DIO29_PWM nullptr + +#define DIO30_PIN PINA1 +#define DIO30_RPORT PINA +#define DIO30_WPORT PORTA +#define DIO30_DDR DDRA +#define DIO30_PWM nullptr + +#define DIO31_PIN PINA0 +#define DIO31_RPORT PINA +#define DIO31_WPORT PORTA +#define DIO31_DDR DDRA +#define DIO31_PWM nullptr + +#define AIO0_PIN PINA0 +#define AIO0_RPORT PINA +#define AIO0_WPORT PORTA +#define AIO0_DDR DDRA +#define AIO0_PWM nullptr + +#define AIO1_PIN PINA1 +#define AIO1_RPORT PINA +#define AIO1_WPORT PORTA +#define AIO1_DDR DDRA +#define AIO1_PWM nullptr + +#define AIO2_PIN PINA2 +#define AIO2_RPORT PINA +#define AIO2_WPORT PORTA +#define AIO2_DDR DDRA +#define AIO2_PWM nullptr + +#define AIO3_PIN PINA3 +#define AIO3_RPORT PINA +#define AIO3_WPORT PORTA +#define AIO3_DDR DDRA +#define AIO3_PWM nullptr + +#define AIO4_PIN PINA4 +#define AIO4_RPORT PINA +#define AIO4_WPORT PORTA +#define AIO4_DDR DDRA +#define AIO4_PWM nullptr + +#define AIO5_PIN PINA5 +#define AIO5_RPORT PINA +#define AIO5_WPORT PORTA +#define AIO5_DDR DDRA +#define AIO5_PWM nullptr + +#define AIO6_PIN PINA6 +#define AIO6_RPORT PINA +#define AIO6_WPORT PORTA +#define AIO6_DDR DDRA +#define AIO6_PWM nullptr + +#define AIO7_PIN PINA7 +#define AIO7_RPORT PINA +#define AIO7_WPORT PORTA +#define AIO7_DDR DDRA +#define AIO7_PWM nullptr + +#undef PA0 +#define PA0_PIN PINA0 +#define PA0_RPORT PINA +#define PA0_WPORT PORTA +#define PA0_DDR DDRA +#define PA0_PWM nullptr + +#undef PA1 +#define PA1_PIN PINA1 +#define PA1_RPORT PINA +#define PA1_WPORT PORTA +#define PA1_DDR DDRA +#define PA1_PWM nullptr + +#undef PA2 +#define PA2_PIN PINA2 +#define PA2_RPORT PINA +#define PA2_WPORT PORTA +#define PA2_DDR DDRA +#define PA2_PWM nullptr + +#undef PA3 +#define PA3_PIN PINA3 +#define PA3_RPORT PINA +#define PA3_WPORT PORTA +#define PA3_DDR DDRA +#define PA3_PWM nullptr + +#undef PA4 +#define PA4_PIN PINA4 +#define PA4_RPORT PINA +#define PA4_WPORT PORTA +#define PA4_DDR DDRA +#define PA4_PWM nullptr + +#undef PA5 +#define PA5_PIN PINA5 +#define PA5_RPORT PINA +#define PA5_WPORT PORTA +#define PA5_DDR DDRA +#define PA5_PWM nullptr + +#undef PA6 +#define PA6_PIN PINA6 +#define PA6_RPORT PINA +#define PA6_WPORT PORTA +#define PA6_DDR DDRA +#define PA6_PWM nullptr + +#undef PA7 +#define PA7_PIN PINA7 +#define PA7_RPORT PINA +#define PA7_WPORT PORTA +#define PA7_DDR DDRA +#define PA7_PWM nullptr + +#undef PB0 +#define PB0_PIN PINB0 +#define PB0_RPORT PINB +#define PB0_WPORT PORTB +#define PB0_DDR DDRB +#define PB0_PWM nullptr + +#undef PB1 +#define PB1_PIN PINB1 +#define PB1_RPORT PINB +#define PB1_WPORT PORTB +#define PB1_DDR DDRB +#define PB1_PWM nullptr + +#undef PB2 +#define PB2_PIN PINB2 +#define PB2_RPORT PINB +#define PB2_WPORT PORTB +#define PB2_DDR DDRB +#define PB2_PWM nullptr + +#undef PB3 +#define PB3_PIN PINB3 +#define PB3_RPORT PINB +#define PB3_WPORT PORTB +#define PB3_DDR DDRB +#define PB3_PWM &OCR0A + +#undef PB4 +#define PB4_PIN PINB4 +#define PB4_RPORT PINB +#define PB4_WPORT PORTB +#define PB4_DDR DDRB +#define PB4_PWM &OCR0B + +#undef PB5 +#define PB5_PIN PINB5 +#define PB5_RPORT PINB +#define PB5_WPORT PORTB +#define PB5_DDR DDRB +#define PB5_PWM nullptr + +#undef PB6 +#define PB6_PIN PINB6 +#define PB6_RPORT PINB +#define PB6_WPORT PORTB +#define PB6_DDR DDRB +#define PB6_PWM nullptr + +#undef PB7 +#define PB7_PIN PINB7 +#define PB7_RPORT PINB +#define PB7_WPORT PORTB +#define PB7_DDR DDRB +#define PB7_PWM nullptr + +#undef PC0 +#define PC0_PIN PINC0 +#define PC0_RPORT PINC +#define PC0_WPORT PORTC +#define PC0_DDR DDRC +#define PC0_PWM nullptr + +#undef PC1 +#define PC1_PIN PINC1 +#define PC1_RPORT PINC +#define PC1_WPORT PORTC +#define PC1_DDR DDRC +#define PC1_PWM nullptr + +#undef PC2 +#define PC2_PIN PINC2 +#define PC2_RPORT PINC +#define PC2_WPORT PORTC +#define PC2_DDR DDRC +#define PC2_PWM nullptr + +#undef PC3 +#define PC3_PIN PINC3 +#define PC3_RPORT PINC +#define PC3_WPORT PORTC +#define PC3_DDR DDRC +#define PC3_PWM nullptr + +#undef PC4 +#define PC4_PIN PINC4 +#define PC4_RPORT PINC +#define PC4_WPORT PORTC +#define PC4_DDR DDRC +#define PC4_PWM nullptr + +#undef PC5 +#define PC5_PIN PINC5 +#define PC5_RPORT PINC +#define PC5_WPORT PORTC +#define PC5_DDR DDRC +#define PC5_PWM nullptr + +#undef PC6 +#define PC6_PIN PINC6 +#define PC6_RPORT PINC +#define PC6_WPORT PORTC +#define PC6_DDR DDRC +#define PC6_PWM nullptr + +#undef PC7 +#define PC7_PIN PINC7 +#define PC7_RPORT PINC +#define PC7_WPORT PORTC +#define PC7_DDR DDRC +#define PC7_PWM nullptr + +#undef PD0 +#define PD0_PIN PIND0 +#define PD0_RPORT PIND +#define PD0_WPORT PORTD +#define PD0_DDR DDRD +#define PD0_PWM nullptr + +#undef PD1 +#define PD1_PIN PIND1 +#define PD1_RPORT PIND +#define PD1_WPORT PORTD +#define PD1_DDR DDRD +#define PD1_PWM nullptr + +#undef PD2 +#define PD2_PIN PIND2 +#define PD2_RPORT PIND +#define PD2_WPORT PORTD +#define PD2_DDR DDRD +#define PD2_PWM nullptr + +#undef PD3 +#define PD3_PIN PIND3 +#define PD3_RPORT PIND +#define PD3_WPORT PORTD +#define PD3_DDR DDRD +#define PD3_PWM nullptr + +#undef PD4 +#define PD4_PIN PIND4 +#define PD4_RPORT PIND +#define PD4_WPORT PORTD +#define PD4_DDR DDRD +#define PD4_PWM nullptr + +#undef PD5 +#define PD5_PIN PIND5 +#define PD5_RPORT PIND +#define PD5_WPORT PORTD +#define PD5_DDR DDRD +#define PD5_PWM nullptr + +#undef PD6 +#define PD6_PIN PIND6 +#define PD6_RPORT PIND +#define PD6_WPORT PORTD +#define PD6_DDR DDRD +#define PD6_PWM &OCR2B + +#undef PD7 +#define PD7_PIN PIND7 +#define PD7_RPORT PIND +#define PD7_WPORT PORTD +#define PD7_DDR DDRD +#define PD7_PWM &OCR2A diff --git a/src/HAL/AVR/fastio/fastio_AT90USB.h b/src/HAL/AVR/fastio/fastio_AT90USB.h new file mode 100644 index 0000000..51d400b --- /dev/null +++ b/src/HAL/AVR/fastio/fastio_AT90USB.h @@ -0,0 +1,697 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Pin mapping (Teensy) for AT90USB646, 647, 1286, and 1287 + * + * Logical Pin: 28 29 30 31 32 33 34 35 20 21 22 23 24 25 26 27 10 11 12 13 14 15 16 17 00 01 02 03 04 05 06 07 08 09(46*47)36 37 18 19 38 39 40 41 42 43 44 45 + * Port: A0 A1 A2 A3 A4 A5 A6 A7 B0 B1 B2 B3 B4 B5 B6 B7 C0 C1 C2 C3 C4 C5 C6 C7 D0 D1 D2 D3 D4 D5 D6 D7 E0 E1 E2 E3 E4 E5 E6 E7 F0 F1 F2 F3 F4 F5 F6 F7 + * The logical pins 46 and 47 are not supported by Teensyduino, but are supported below as E2 and E3 + */ + +#include "../fastio.h" + +// change for your board +#define DEBUG_LED DIO31 /* led D5 red */ + +// SPI +#define SCK DIO21 // 9 +#define MISO DIO23 // 11 +#define MOSI DIO22 // 10 +#define SS DIO20 // 8 + +// Digital I/O + +#define DIO0_PIN PIND0 +#define DIO0_RPORT PIND +#define DIO0_WPORT PORTD +#define DIO0_PWM 0 +#define DIO0_DDR DDRD + +#define DIO1_PIN PIND1 +#define DIO1_RPORT PIND +#define DIO1_WPORT PORTD +#define DIO1_PWM 0 +#define DIO1_DDR DDRD + +#define DIO2_PIN PIND2 +#define DIO2_RPORT PIND +#define DIO2_WPORT PORTD +#define DIO2_PWM 0 +#define DIO2_DDR DDRD + +#define DIO3_PIN PIND3 +#define DIO3_RPORT PIND +#define DIO3_WPORT PORTD +#define DIO3_PWM 0 +#define DIO3_DDR DDRD + +#define DIO4_PIN PIND4 +#define DIO4_RPORT PIND +#define DIO4_WPORT PORTD +#define DIO4_PWM 0 +#define DIO4_DDR DDRD + +#define DIO5_PIN PIND5 +#define DIO5_RPORT PIND +#define DIO5_WPORT PORTD +#define DIO5_PWM 0 +#define DIO5_DDR DDRD + +#define DIO6_PIN PIND6 +#define DIO6_RPORT PIND +#define DIO6_WPORT PORTD +#define DIO6_PWM 0 +#define DIO6_DDR DDRD + +#define DIO7_PIN PIND7 +#define DIO7_RPORT PIND +#define DIO7_WPORT PORTD +#define DIO7_PWM 0 +#define DIO7_DDR DDRD + +#define DIO8_PIN PINE0 +#define DIO8_RPORT PINE +#define DIO8_WPORT PORTE +#define DIO8_PWM 0 +#define DIO8_DDR DDRE + +#define DIO9_PIN PINE1 +#define DIO9_RPORT PINE +#define DIO9_WPORT PORTE +#define DIO9_PWM 0 +#define DIO9_DDR DDRE + +#define DIO10_PIN PINC0 +#define DIO10_RPORT PINC +#define DIO10_WPORT PORTC +#define DIO10_PWM 0 +#define DIO10_DDR DDRC + +#define DIO11_PIN PINC1 +#define DIO11_RPORT PINC +#define DIO11_WPORT PORTC +#define DIO11_PWM 0 +#define DIO11_DDR DDRC + +#define DIO12_PIN PINC2 +#define DIO12_RPORT PINC +#define DIO12_WPORT PORTC +#define DIO12_PWM 0 +#define DIO12_DDR DDRC + +#define DIO13_PIN PINC3 +#define DIO13_RPORT PINC +#define DIO13_WPORT PORTC +#define DIO13_PWM 0 +#define DIO13_DDR DDRC + +#define DIO14_PIN PINC4 +#define DIO14_RPORT PINC +#define DIO14_WPORT PORTC +#define DIO14_PWM 0 // OC3C +#define DIO14_DDR DDRC + +#define DIO15_PIN PINC5 +#define DIO15_RPORT PINC +#define DIO15_WPORT PORTC +#define DIO15_PWM 0 // OC3B +#define DIO15_DDR DDRC + +#define DIO16_PIN PINC6 +#define DIO16_RPORT PINC +#define DIO16_WPORT PORTC +#define DIO16_PWM 0 // OC3A +#define DIO16_DDR DDRC + +#define DIO17_PIN PINC7 +#define DIO17_RPORT PINC +#define DIO17_WPORT PORTC +#define DIO17_PWM 0 +#define DIO17_DDR DDRC + +#define DIO18_PIN PINE6 +#define DIO18_RPORT PINE +#define DIO18_WPORT PORTE +#define DIO18_PWM 0 +#define DIO18_DDR DDRE + +#define DIO19_PIN PINE7 +#define DIO19_RPORT PINE +#define DIO19_WPORT PORTE +#define DIO19_PWM 0 +#define DIO19_DDR DDRE + +#define DIO20_PIN PINB0 +#define DIO20_RPORT PINB +#define DIO20_WPORT PORTB +#define DIO20_PWM 0 +#define DIO20_DDR DDRB + +#define DIO21_PIN PINB1 +#define DIO21_RPORT PINB +#define DIO21_WPORT PORTB +#define DIO21_PWM 0 +#define DIO21_DDR DDRB + +#define DIO22_PIN PINB2 +#define DIO22_RPORT PINB +#define DIO22_WPORT PORTB +#define DIO22_PWM 0 +#define DIO22_DDR DDRB + +#define DIO23_PIN PINB3 +#define DIO23_RPORT PINB +#define DIO23_WPORT PORTB +#define DIO23_PWM 0 +#define DIO23_DDR DDRB + +#define DIO24_PIN PINB4 +#define DIO24_RPORT PINB +#define DIO24_WPORT PORTB +#define DIO24_PWM 0 // OC2A +#define DIO24_DDR DDRB + +#define DIO25_PIN PINB5 +#define DIO25_RPORT PINB +#define DIO25_WPORT PORTB +#define DIO25_PWM 0 // OC1A +#define DIO25_DDR DDRB + +#define DIO26_PIN PINB6 +#define DIO26_RPORT PINB +#define DIO26_WPORT PORTB +#define DIO26_PWM 0 // OC1B +#define DIO26_DDR DDRB + +#define DIO27_PIN PINB7 +#define DIO27_RPORT PINB +#define DIO27_WPORT PORTB +#define DIO27_PWM 0 // OC1C +#define DIO27_DDR DDRB + +#define DIO28_PIN PINA0 +#define DIO28_RPORT PINA +#define DIO28_WPORT PORTA +#define DIO28_PWM 0 +#define DIO28_DDR DDRA + +#define DIO29_PIN PINA1 +#define DIO29_RPORT PINA +#define DIO29_WPORT PORTA +#define DIO29_PWM 0 +#define DIO29_DDR DDRA + +#define DIO30_PIN PINA2 +#define DIO30_RPORT PINA +#define DIO30_WPORT PORTA +#define DIO30_PWM 0 +#define DIO30_DDR DDRA + +#define DIO31_PIN PINA3 +#define DIO31_RPORT PINA +#define DIO31_WPORT PORTA +#define DIO31_PWM 0 +#define DIO31_DDR DDRA + +#define DIO32_PIN PINA4 +#define DIO32_RPORT PINA +#define DIO32_WPORT PORTA +#define DIO32_PWM 0 +#define DIO32_DDR DDRA + +#define DIO33_PIN PINA5 +#define DIO33_RPORT PINA +#define DIO33_WPORT PORTA +#define DIO33_PWM 0 +#define DIO33_DDR DDRA + +#define DIO34_PIN PINA6 +#define DIO34_RPORT PINA +#define DIO34_WPORT PORTA +#define DIO34_PWM 0 +#define DIO34_DDR DDRA + +#define DIO35_PIN PINA7 +#define DIO35_RPORT PINA +#define DIO35_WPORT PORTA +#define DIO35_PWM 0 +#define DIO35_DDR DDRA + +#define DIO36_PIN PINE4 +#define DIO36_RPORT PINE +#define DIO36_WPORT PORTE +#define DIO36_PWM 0 +#define DIO36_DDR DDRE + +#define DIO37_PIN PINE5 +#define DIO37_RPORT PINE +#define DIO37_WPORT PORTE +#define DIO37_PWM 0 +#define DIO37_DDR DDRE + +#define DIO38_PIN PINF0 +#define DIO38_RPORT PINF +#define DIO38_WPORT PORTF +#define DIO38_PWM 0 +#define DIO38_DDR DDRF + +#define DIO39_PIN PINF1 +#define DIO39_RPORT PINF +#define DIO39_WPORT PORTF +#define DIO39_PWM 0 +#define DIO39_DDR DDRF + +#define DIO40_PIN PINF2 +#define DIO40_RPORT PINF +#define DIO40_WPORT PORTF +#define DIO40_PWM 0 +#define DIO40_DDR DDRF + +#define DIO41_PIN PINF3 +#define DIO41_RPORT PINF +#define DIO41_WPORT PORTF +#define DIO41_PWM 0 +#define DIO41_DDR DDRF + +#define DIO42_PIN PINF4 +#define DIO42_RPORT PINF +#define DIO42_WPORT PORTF +#define DIO42_PWM 0 +#define DIO42_DDR DDRF + +#define DIO43_PIN PINF5 +#define DIO43_RPORT PINF +#define DIO43_WPORT PORTF +#define DIO43_PWM 0 +#define DIO43_DDR DDRF + +#define DIO44_PIN PINF6 +#define DIO44_RPORT PINF +#define DIO44_WPORT PORTF +#define DIO44_PWM 0 +#define DIO44_DDR DDRF + +#define DIO45_PIN PINF7 +#define DIO45_RPORT PINF +#define DIO45_WPORT PORTF +#define DIO45_PWM 0 +#define DIO45_DDR DDRF + +#define AIO0_PIN PINF0 +#define AIO0_RPORT PINF +#define AIO0_WPORT PORTF +#define AIO0_PWM 0 +#define AIO0_DDR DDRF + +#define AIO1_PIN PINF1 +#define AIO1_RPORT PINF +#define AIO1_WPORT PORTF +#define AIO1_PWM 0 +#define AIO1_DDR DDRF + +#define AIO2_PIN PINF2 +#define AIO2_RPORT PINF +#define AIO2_WPORT PORTF +#define AIO2_PWM 0 +#define AIO2_DDR DDRF + +#define AIO3_PIN PINF3 +#define AIO3_RPORT PINF +#define AIO3_WPORT PORTF +#define AIO3_PWM 0 +#define AIO3_DDR DDRF + +#define AIO4_PIN PINF4 +#define AIO4_RPORT PINF +#define AIO4_WPORT PORTF +#define AIO4_PWM 0 +#define AIO4_DDR DDRF + +#define AIO5_PIN PINF5 +#define AIO5_RPORT PINF +#define AIO5_WPORT PORTF +#define AIO5_PWM 0 +#define AIO5_DDR DDRF + +#define AIO6_PIN PINF6 +#define AIO6_RPORT PINF +#define AIO6_WPORT PORTF +#define AIO6_PWM 0 +#define AIO6_DDR DDRF + +#define AIO7_PIN PINF7 +#define AIO7_RPORT PINF +#define AIO7_WPORT PORTF +#define AIO7_PWM 0 +#define AIO7_DDR DDRF + +//-- Begin not supported by Teensyduino +//-- don't use Arduino functions on these pins pinMode/digitalWrite/etc +#define DIO46_PIN PINE2 +#define DIO46_RPORT PINE +#define DIO46_WPORT PORTE +#define DIO46_PWM 0 +#define DIO46_DDR DDRE + +#define DIO47_PIN PINE3 +#define DIO47_RPORT PINE +#define DIO47_WPORT PORTE +#define DIO47_PWM 0 +#define DIO47_DDR DDRE + +#define TEENSY_E2 46 +#define TEENSY_E3 47 + +//-- end not supported by Teensyduino + +#undef PA0 +#define PA0_PIN PINA0 +#define PA0_RPORT PINA +#define PA0_WPORT PORTA +#define PA0_PWM 0 +#define PA0_DDR DDRA +#undef PA1 +#define PA1_PIN PINA1 +#define PA1_RPORT PINA +#define PA1_WPORT PORTA +#define PA1_PWM 0 +#define PA1_DDR DDRA +#undef PA2 +#define PA2_PIN PINA2 +#define PA2_RPORT PINA +#define PA2_WPORT PORTA +#define PA2_PWM 0 +#define PA2_DDR DDRA +#undef PA3 +#define PA3_PIN PINA3 +#define PA3_RPORT PINA +#define PA3_WPORT PORTA +#define PA3_PWM 0 +#define PA3_DDR DDRA +#undef PA4 +#define PA4_PIN PINA4 +#define PA4_RPORT PINA +#define PA4_WPORT PORTA +#define PA4_PWM 0 +#define PA4_DDR DDRA +#undef PA5 +#define PA5_PIN PINA5 +#define PA5_RPORT PINA +#define PA5_WPORT PORTA +#define PA5_PWM 0 +#define PA5_DDR DDRA +#undef PA6 +#define PA6_PIN PINA6 +#define PA6_RPORT PINA +#define PA6_WPORT PORTA +#define PA6_PWM 0 +#define PA6_DDR DDRA +#undef PA7 +#define PA7_PIN PINA7 +#define PA7_RPORT PINA +#define PA7_WPORT PORTA +#define PA7_PWM 0 +#define PA7_DDR DDRA + +#undef PB0 +#define PB0_PIN PINB0 +#define PB0_RPORT PINB +#define PB0_WPORT PORTB +#define PB0_PWM 0 +#define PB0_DDR DDRB +#undef PB1 +#define PB1_PIN PINB1 +#define PB1_RPORT PINB +#define PB1_WPORT PORTB +#define PB1_PWM 0 +#define PB1_DDR DDRB +#undef PB2 +#define PB2_PIN PINB2 +#define PB2_RPORT PINB +#define PB2_WPORT PORTB +#define PB2_PWM 0 +#define PB2_DDR DDRB +#undef PB3 +#define PB3_PIN PINB3 +#define PB3_RPORT PINB +#define PB3_WPORT PORTB +#define PB3_PWM 0 +#define PB3_DDR DDRB +#undef PB4 +#define PB4_PIN PINB4 +#define PB4_RPORT PINB +#define PB4_WPORT PORTB +#define PB4_PWM 0 +#define PB4_DDR DDRB +#undef PB5 +#define PB5_PIN PINB5 +#define PB5_RPORT PINB +#define PB5_WPORT PORTB +#define PB5_PWM 0 +#define PB5_DDR DDRB +#undef PB6 +#define PB6_PIN PINB6 +#define PB6_RPORT PINB +#define PB6_WPORT PORTB +#define PB6_PWM 0 +#define PB6_DDR DDRB +#undef PB7 +#define PB7_PIN PINB7 +#define PB7_RPORT PINB +#define PB7_WPORT PORTB +#define PB7_PWM 0 +#define PB7_DDR DDRB + +#undef PC0 +#define PC0_PIN PINC0 +#define PC0_RPORT PINC +#define PC0_WPORT PORTC +#define PC0_PWM 0 +#define PC0_DDR DDRC +#undef PC1 +#define PC1_PIN PINC1 +#define PC1_RPORT PINC +#define PC1_WPORT PORTC +#define PC1_PWM 0 +#define PC1_DDR DDRC +#undef PC2 +#define PC2_PIN PINC2 +#define PC2_RPORT PINC +#define PC2_WPORT PORTC +#define PC2_PWM 0 +#define PC2_DDR DDRC +#undef PC3 +#define PC3_PIN PINC3 +#define PC3_RPORT PINC +#define PC3_WPORT PORTC +#define PC3_PWM 0 +#define PC3_DDR DDRC +#undef PC4 +#define PC4_PIN PINC4 +#define PC4_RPORT PINC +#define PC4_WPORT PORTC +#define PC4_PWM 0 +#define PC4_DDR DDRC +#undef PC5 +#define PC5_PIN PINC5 +#define PC5_RPORT PINC +#define PC5_WPORT PORTC +#define PC5_PWM 0 +#define PC5_DDR DDRC +#undef PC6 +#define PC6_PIN PINC6 +#define PC6_RPORT PINC +#define PC6_WPORT PORTC +#define PC6_PWM 0 +#define PC6_DDR DDRC +#undef PC7 +#define PC7_PIN PINC7 +#define PC7_RPORT PINC +#define PC7_WPORT PORTC +#define PC7_PWM 0 +#define PC7_DDR DDRC + +#undef PD0 +#define PD0_PIN PIND0 +#define PD0_RPORT PIND +#define PD0_WPORT PORTD +#define PD0_PWM 0 // OC0B +#define PD0_DDR DDRD +#undef PD1 +#define PD1_PIN PIND1 +#define PD1_RPORT PIND +#define PD1_WPORT PORTD +#define PD1_PWM 0 // OC2B +#define PD1_DDR DDRD +#undef PD2 +#define PD2_PIN PIND2 +#define PD2_RPORT PIND +#define PD2_WPORT PORTD +#define PD2_PWM 0 +#define PD2_DDR DDRD +#undef PD3 +#define PD3_PIN PIND3 +#define PD3_RPORT PIND +#define PD3_WPORT PORTD +#define PD3_PWM 0 +#define PD3_DDR DDRD +#undef PD4 +#define PD4_PIN PIND4 +#define PD4_RPORT PIND +#define PD4_WPORT PORTD +#define PD4_PWM 0 +#define PD4_DDR DDRD +#undef PD5 +#define PD5_PIN PIND5 +#define PD5_RPORT PIND +#define PD5_WPORT PORTD +#define PD5_PWM 0 +#define PD5_DDR DDRD +#undef PD6 +#define PD6_PIN PIND6 +#define PD6_RPORT PIND +#define PD6_WPORT PORTD +#define PD6_PWM 0 +#define PD6_DDR DDRD +#undef PD7 +#define PD7_PIN PIND7 +#define PD7_RPORT PIND +#define PD7_WPORT PORTD +#define PD7_PWM 0 +#define PD7_DDR DDRD + +#undef PE0 +#define PE0_PIN PINE0 +#define PE0_RPORT PINE +#define PE0_WPORT PORTE +#define PE0_PWM 0 +#define PE0_DDR DDRE +#undef PE1 +#define PE1_PIN PINE1 +#define PE1_RPORT PINE +#define PE1_WPORT PORTE +#define PE1_PWM 0 +#define PE1_DDR DDRE +#undef PE2 +#define PE2_PIN PINE2 +#define PE2_RPORT PINE +#define PE2_WPORT PORTE +#define PE2_PWM 0 +#define PE2_DDR DDRE +#undef PE3 +#define PE3_PIN PINE3 +#define PE3_RPORT PINE +#define PE3_WPORT PORTE +#define PE3_PWM 0 +#define PE3_DDR DDRE +#undef PE4 +#define PE4_PIN PINE4 +#define PE4_RPORT PINE +#define PE4_WPORT PORTE +#define PE4_PWM 0 +#define PE4_DDR DDRE +#undef PE5 +#define PE5_PIN PINE5 +#define PE5_RPORT PINE +#define PE5_WPORT PORTE +#define PE5_PWM 0 +#define PE5_DDR DDRE +#undef PE6 +#define PE6_PIN PINE6 +#define PE6_RPORT PINE +#define PE6_WPORT PORTE +#define PE6_PWM 0 +#define PE6_DDR DDRE +#undef PE7 +#define PE7_PIN PINE7 +#define PE7_RPORT PINE +#define PE7_WPORT PORTE +#define PE7_PWM 0 +#define PE7_DDR DDRE + +#undef PF0 +#define PF0_PIN PINF0 +#define PF0_RPORT PINF +#define PF0_WPORT PORTF +#define PF0_PWM 0 +#define PF0_DDR DDRF +#undef PF1 +#define PF1_PIN PINF1 +#define PF1_RPORT PINF +#define PF1_WPORT PORTF +#define PF1_PWM 0 +#define PF1_DDR DDRF +#undef PF2 +#define PF2_PIN PINF2 +#define PF2_RPORT PINF +#define PF2_WPORT PORTF +#define PF2_PWM 0 +#define PF2_DDR DDRF +#undef PF3 +#define PF3_PIN PINF3 +#define PF3_RPORT PINF +#define PF3_WPORT PORTF +#define PF3_PWM 0 +#define PF3_DDR DDRF +#undef PF4 +#define PF4_PIN PINF4 +#define PF4_RPORT PINF +#define PF4_WPORT PORTF +#define PF4_PWM 0 +#define PF4_DDR DDRF +#undef PF5 +#define PF5_PIN PINF5 +#define PF5_RPORT PINF +#define PF5_WPORT PORTF +#define PF5_PWM 0 +#define PF5_DDR DDRF +#undef PF6 +#define PF6_PIN PINF6 +#define PF6_RPORT PINF +#define PF6_WPORT PORTF +#define PF6_PWM 0 +#define PF6_DDR DDRF +#undef PF7 +#define PF7_PIN PINF7 +#define PF7_RPORT PINF +#define PF7_WPORT PORTF +#define PF7_PWM 0 +#define PF7_DDR DDRF + + +/** + * Some of the pin mapping functions of the Teensduino extension to the Arduino IDE + * do not function the same as the other Arduino extensions. + */ + +//digitalPinToTimer(pin) function works like Arduino but Timers are not defined +#define TIMER0B 1 +#define TIMER1A 7 +#define TIMER1B 8 +#define TIMER1C 9 +#define TIMER2A 6 +#define TIMER2B 2 +#define TIMER3A 5 +#define TIMER3B 4 +#define TIMER3C 3 diff --git a/src/HAL/AVR/inc/Conditionals_LCD.h b/src/HAL/AVR/inc/Conditionals_LCD.h new file mode 100644 index 0000000..a611ccd --- /dev/null +++ b/src/HAL/AVR/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/AVR." +#endif diff --git a/src/HAL/AVR/inc/Conditionals_adv.h b/src/HAL/AVR/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/AVR/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/AVR/inc/Conditionals_post.h b/src/HAL/AVR/inc/Conditionals_post.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/AVR/inc/Conditionals_post.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/AVR/inc/SanityCheck.h b/src/HAL/AVR/inc/SanityCheck.h new file mode 100644 index 0000000..1c1d8d4 --- /dev/null +++ b/src/HAL/AVR/inc/SanityCheck.h @@ -0,0 +1,101 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test AVR-specific configuration values for errors at compile-time. + */ + +/** + * Check for common serial pin conflicts + */ +#define CHECK_SERIAL_PIN(N) ( \ + X_STOP_PIN == N || Y_STOP_PIN == N || Z_STOP_PIN == N \ + || X_MIN_PIN == N || Y_MIN_PIN == N || Z_MIN_PIN == N \ + || X_MAX_PIN == N || Y_MAX_PIN == N || Z_MAX_PIN == N \ + || X_STEP_PIN == N || Y_STEP_PIN == N || Z_STEP_PIN == N \ + || X_DIR_PIN == N || Y_DIR_PIN == N || Z_DIR_PIN == N \ + || X_ENA_PIN == N || Y_ENA_PIN == N || Z_ENA_PIN == N \ +) +#if CONF_SERIAL_IS(0) // D0-D1. No known conflicts. +#endif +#if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19)) + #error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board." +#endif +#if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17)) + #error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board." +#endif +#if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15)) + #error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board." +#endif +#undef CHECK_SERIAL_PIN + +/** + * Checks for FAST PWM + */ +#if ALL(FAST_PWM_FAN, USE_OCR2A_AS_TOP, HAS_TCCR2) + #error "USE_OCR2A_AS_TOP does not apply to devices with a single output TIMER2." +#endif + +/** + * Checks for SOFT PWM + */ +#if HAS_FAN0 && FAN_PIN == 9 && DISABLED(FAN_SOFT_PWM) && ENABLED(SPEAKER) + #error "FAN_PIN 9 Hardware PWM uses Timer 2 which conflicts with Arduino AVR Tone Timer (for SPEAKER)." + #error "Disable SPEAKER or enable FAN_SOFT_PWM." +#endif + +/** + * Sanity checks for Spindle / Laser PWM + */ +#if ENABLED(SPINDLE_LASER_USE_PWM) + #include "../ServoTimers.h" // Needed to check timer availability (_useTimer3) + #if SPINDLE_LASER_PWM_PIN == 4 || WITHIN(SPINDLE_LASER_PWM_PIN, 11, 13) + #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by a system interrupt." + #elif NUM_SERVOS > 0 && defined(_useTimer3) && (WITHIN(SPINDLE_LASER_PWM_PIN, 2, 3) || SPINDLE_LASER_PWM_PIN == 5) + #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by the servo system." + #endif +#elif SPINDLE_LASER_FREQUENCY + #error "SPINDLE_LASER_FREQUENCY requires SPINDLE_LASER_USE_PWM." +#endif + +/** + * The Trinamic library includes SoftwareSerial.h, leading to a compile error. + */ +#if BOTH(HAS_TRINAMIC_CONFIG, ENDSTOP_INTERRUPTS_FEATURE) + #error "TMCStepper includes SoftwareSerial.h which is incompatible with ENDSTOP_INTERRUPTS_FEATURE. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." +#endif + +#if BOTH(HAS_TMC_SW_SERIAL, MONITOR_DRIVER_STATUS) + #error "MONITOR_DRIVER_STATUS causes performance issues when used with SoftwareSerial-connected drivers. Disable MONITOR_DRIVER_STATUS or use hardware serial to continue." +#endif + +/** + * Postmortem debugging + */ +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not supported on AVR boards." +#endif + +#if USING_PULLDOWNS + #error "PULLDOWN pin mode is not available on AVR boards." +#endif diff --git a/src/HAL/AVR/math.h b/src/HAL/AVR/math.h new file mode 100644 index 0000000..7dd1018 --- /dev/null +++ b/src/HAL/AVR/math.h @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Optimized math functions for AVR + */ + +// intRes = longIn1 * longIn2 >> 24 +// uses: +// A[tmp] to store 0 +// B[tmp] to store bits 16-23 of the 48bit result. The top bit is used to round the two byte result. +// note that the lower two bytes and the upper byte of the 48bit result are not calculated. +// this can cause the result to be out by one as the lower bytes may cause carries into the upper ones. +// B A are bits 24-39 and are the returned value +// C B A is longIn1 +// D C B A is longIn2 +// +FORCE_INLINE static uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) { + uint8_t tmp1; + uint8_t tmp2; + uint16_t intRes; + __asm__ __volatile__( + A("clr %[tmp1]") + A("mul %A[longIn1], %B[longIn2]") + A("mov %[tmp2], r1") + A("mul %B[longIn1], %C[longIn2]") + A("movw %A[intRes], r0") + A("mul %C[longIn1], %C[longIn2]") + A("add %B[intRes], r0") + A("mul %C[longIn1], %B[longIn2]") + A("add %A[intRes], r0") + A("adc %B[intRes], r1") + A("mul %A[longIn1], %C[longIn2]") + A("add %[tmp2], r0") + A("adc %A[intRes], r1") + A("adc %B[intRes], %[tmp1]") + A("mul %B[longIn1], %B[longIn2]") + A("add %[tmp2], r0") + A("adc %A[intRes], r1") + A("adc %B[intRes], %[tmp1]") + A("mul %C[longIn1], %A[longIn2]") + A("add %[tmp2], r0") + A("adc %A[intRes], r1") + A("adc %B[intRes], %[tmp1]") + A("mul %B[longIn1], %A[longIn2]") + A("add %[tmp2], r1") + A("adc %A[intRes], %[tmp1]") + A("adc %B[intRes], %[tmp1]") + A("lsr %[tmp2]") + A("adc %A[intRes], %[tmp1]") + A("adc %B[intRes], %[tmp1]") + A("mul %D[longIn2], %A[longIn1]") + A("add %A[intRes], r0") + A("adc %B[intRes], r1") + A("mul %D[longIn2], %B[longIn1]") + A("add %B[intRes], r0") + A("clr r1") + : [intRes] "=&r" (intRes), + [tmp1] "=&r" (tmp1), + [tmp2] "=&r" (tmp2) + : [longIn1] "d" (longIn1), + [longIn2] "d" (longIn2) + : "cc" + ); + return intRes; +} + +// intRes = intIn1 * intIn2 >> 16 +// uses: +// r26 to store 0 +// r27 to store the byte 1 of the 24 bit result +FORCE_INLINE static uint16_t MultiU16X8toH16(uint8_t charIn1, uint16_t intIn2) { + uint8_t tmp; + uint16_t intRes; + __asm__ __volatile__ ( + A("clr %[tmp]") + A("mul %[charIn1], %B[intIn2]") + A("movw %A[intRes], r0") + A("mul %[charIn1], %A[intIn2]") + A("add %A[intRes], r1") + A("adc %B[intRes], %[tmp]") + A("lsr r0") + A("adc %A[intRes], %[tmp]") + A("adc %B[intRes], %[tmp]") + A("clr r1") + : [intRes] "=&r" (intRes), + [tmp] "=&r" (tmp) + : [charIn1] "d" (charIn1), + [intIn2] "d" (intIn2) + : "cc" + ); + return intRes; +} diff --git a/src/HAL/AVR/pinsDebug.h b/src/HAL/AVR/pinsDebug.h new file mode 100644 index 0000000..dab4e44 --- /dev/null +++ b/src/HAL/AVR/pinsDebug.h @@ -0,0 +1,400 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * PWM print routines for Atmel 8 bit AVR CPUs + */ + +#include "../../inc/MarlinConfig.h" + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS + +#if MB(BQ_ZUM_MEGA_3D, MIGHTYBOARD_REVE, MINIRAMBO, SCOOVO_X9H, TRIGORILLA_14) + #define AVR_ATmega2560_FAMILY_PLUS_70 1 +#endif + +#if AVR_AT90USB1286_FAMILY + + // Working with Teensyduino extension so need to re-define some things + #include "pinsDebug_Teensyduino.h" + // Can't use the "digitalPinToPort" function from the Teensyduino type IDEs + // portModeRegister takes a different argument + #define digitalPinToTimer_DEBUG(p) digitalPinToTimer(p) + #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask(p) + #define digitalPinToPort_DEBUG(p) digitalPinToPort(p) + #define GET_PINMODE(pin) (*portModeRegister(pin) & digitalPinToBitMask_DEBUG(pin)) + +#elif AVR_ATmega2560_FAMILY_PLUS_70 // So we can access/display all the pins on boards using more than 70 + + #include "pinsDebug_plus_70.h" + #define digitalPinToTimer_DEBUG(p) digitalPinToTimer_plus_70(p) + #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask_plus_70(p) + #define digitalPinToPort_DEBUG(p) digitalPinToPort_plus_70(p) + bool GET_PINMODE(int8_t pin) {return *portModeRegister(digitalPinToPort_DEBUG(pin)) & digitalPinToBitMask_DEBUG(pin); } + +#else + + #define digitalPinToTimer_DEBUG(p) digitalPinToTimer(p) + #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask(p) + #define digitalPinToPort_DEBUG(p) digitalPinToPort(p) + bool GET_PINMODE(int8_t pin) {return *portModeRegister(digitalPinToPort_DEBUG(pin)) & digitalPinToBitMask_DEBUG(pin); } + #define GET_ARRAY_PIN(p) pgm_read_byte(&pin_array[p].pin) + +#endif + +#define VALID_PIN(pin) (pin >= 0 && pin < NUM_DIGITAL_PINS ? 1 : 0) +#if AVR_ATmega1284_FAMILY + #define DIGITAL_PIN_TO_ANALOG_PIN(P) int(analogInputToDigitalPin(0) - (P)) + #define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(7) && (P) <= analogInputToDigitalPin(0)) +#else + #define DIGITAL_PIN_TO_ANALOG_PIN(P) int((P) - analogInputToDigitalPin(0)) + #define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(0) && ((P) <= analogInputToDigitalPin(15) || (P) <= analogInputToDigitalPin(7))) +#endif +#define GET_ARRAY_PIN(p) pgm_read_byte(&pin_array[p].pin) +#define MULTI_NAME_PAD 26 // space needed to be pretty if not first name assigned to a pin + +void PRINT_ARRAY_NAME(uint8_t x) { + PGM_P const name_mem_pointer = (PGM_P)pgm_read_ptr(&pin_array[x].name); + LOOP_L_N(y, MAX_NAME_LENGTH) { + char temp_char = pgm_read_byte(name_mem_pointer + y); + if (temp_char != 0) + SERIAL_CHAR(temp_char); + else { + LOOP_L_N(i, MAX_NAME_LENGTH - y) SERIAL_CHAR(' '); + break; + } + } +} + +#define GET_ARRAY_IS_DIGITAL(x) pgm_read_byte(&pin_array[x].is_digital) + + +#if defined(__AVR_ATmega1284P__) // 1284 IDE extensions set this to the number of + #undef NUM_DIGITAL_PINS // digital only pins while all other CPUs have it + #define NUM_DIGITAL_PINS 32 // set to digital only + digital/analog +#endif + +#define PWM_PRINT(V) do{ sprintf_P(buffer, PSTR("PWM: %4d"), V); SERIAL_ECHO(buffer); }while(0) +#define PWM_CASE(N,Z) \ + case TIMER##N##Z: \ + if (TCCR##N##A & (_BV(COM##N##Z##1) | _BV(COM##N##Z##0))) { \ + PWM_PRINT(OCR##N##Z); \ + return true; \ + } else return false + +#define ABTEST(N) defined(TCCR##N##A) && defined(COM##N##A1) + +/** + * Print a pin's PWM status. + * Return true if it's currently a PWM pin. + */ +static bool pwm_status(uint8_t pin) { + char buffer[20]; // for the sprintf statements + + switch (digitalPinToTimer_DEBUG(pin)) { + + #if ABTEST(0) + #ifdef TIMER0A + #if !AVR_AT90USB1286_FAMILY // not available in Teensyduino type IDEs + PWM_CASE(0, A); + #endif + #endif + PWM_CASE(0, B); + #endif + + #if ABTEST(1) + PWM_CASE(1, A); + PWM_CASE(1, B); + #if defined(COM1C1) && defined(TIMER1C) + PWM_CASE(1, C); + #endif + #endif + + #if ABTEST(2) + PWM_CASE(2, A); + PWM_CASE(2, B); + #endif + + #if ABTEST(3) + PWM_CASE(3, A); + PWM_CASE(3, B); + #ifdef COM3C1 + PWM_CASE(3, C); + #endif + #endif + + #ifdef TCCR4A + PWM_CASE(4, A); + PWM_CASE(4, B); + PWM_CASE(4, C); + #endif + + #if ABTEST(5) + PWM_CASE(5, A); + PWM_CASE(5, B); + PWM_CASE(5, C); + #endif + + case NOT_ON_TIMER: + default: + return false; + } + SERIAL_ECHO_SP(2); +} // pwm_status + + +const volatile uint8_t* const PWM_other[][3] PROGMEM = { + { &TCCR0A, &TCCR0B, &TIMSK0 }, + { &TCCR1A, &TCCR1B, &TIMSK1 }, + #if ABTEST(2) + { &TCCR2A, &TCCR2B, &TIMSK2 }, + #endif + #if ABTEST(3) + { &TCCR3A, &TCCR3B, &TIMSK3 }, + #endif + #ifdef TCCR4A + { &TCCR4A, &TCCR4B, &TIMSK4 }, + #endif + #if ABTEST(5) + { &TCCR5A, &TCCR5B, &TIMSK5 }, + #endif +}; + + +const volatile uint8_t* const PWM_OCR[][3] PROGMEM = { + + #ifdef TIMER0A + { &OCR0A, &OCR0B, 0 }, + #else + { 0, &OCR0B, 0 }, + #endif + + #if defined(COM1C1) && defined(TIMER1C) + { (const uint8_t*)&OCR1A, (const uint8_t*)&OCR1B, (const uint8_t*)&OCR1C }, + #else + { (const uint8_t*)&OCR1A, (const uint8_t*)&OCR1B, 0 }, + #endif + + #if ABTEST(2) + { &OCR2A, &OCR2B, 0 }, + #endif + + #if ABTEST(3) + #ifdef COM3C1 + { (const uint8_t*)&OCR3A, (const uint8_t*)&OCR3B, (const uint8_t*)&OCR3C }, + #else + { (const uint8_t*)&OCR3A, (const uint8_t*)&OCR3B, 0 }, + #endif + #endif + + #ifdef TCCR4A + { (const uint8_t*)&OCR4A, (const uint8_t*)&OCR4B, (const uint8_t*)&OCR4C }, + #endif + + #if ABTEST(5) + { (const uint8_t*)&OCR5A, (const uint8_t*)&OCR5B, (const uint8_t*)&OCR5C }, + #endif +}; + + +#define TCCR_A(T) pgm_read_word(&PWM_other[T][0]) +#define TCCR_B(T) pgm_read_word(&PWM_other[T][1]) +#define TIMSK(T) pgm_read_word(&PWM_other[T][2]) +#define CS_0 0 +#define CS_1 1 +#define CS_2 2 +#define WGM_0 0 +#define WGM_1 1 +#define WGM_2 3 +#define WGM_3 4 +#define TOIE 0 + +#define OCR_VAL(T, L) pgm_read_word(&PWM_OCR[T][L]) + +static void err_is_counter() { SERIAL_ECHOPGM(" non-standard PWM mode"); } +static void err_is_interrupt() { SERIAL_ECHOPGM(" compare interrupt enabled"); } +static void err_prob_interrupt() { SERIAL_ECHOPGM(" overflow interrupt enabled"); } +static void print_is_also_tied() { SERIAL_ECHOPGM(" is also tied to this pin"); SERIAL_ECHO_SP(14); } + +inline void com_print(const uint8_t N, const uint8_t Z) { + const uint8_t *TCCRA = (uint8_t*)TCCR_A(N); + SERIAL_ECHOPGM(" COM", AS_DIGIT(N)); + SERIAL_CHAR(Z); + SERIAL_ECHOPGM(": ", int((*TCCRA >> (6 - Z * 2)) & 0x03)); +} + +void timer_prefix(uint8_t T, char L, uint8_t N) { // T - timer L - pwm N - WGM bit layout + char buffer[20]; // for the sprintf statements + const uint8_t *TCCRB = (uint8_t*)TCCR_B(T), + *TCCRA = (uint8_t*)TCCR_A(T); + uint8_t WGM = (((*TCCRB & _BV(WGM_2)) >> 1) | (*TCCRA & (_BV(WGM_0) | _BV(WGM_1)))); + if (N == 4) WGM |= ((*TCCRB & _BV(WGM_3)) >> 1); + + SERIAL_ECHOPGM(" TIMER", AS_DIGIT(T)); + SERIAL_CHAR(L); + SERIAL_ECHO_SP(3); + + if (N == 3) { + const uint8_t *OCRVAL8 = (uint8_t*)OCR_VAL(T, L - 'A'); + PWM_PRINT(*OCRVAL8); + } + else { + const uint16_t *OCRVAL16 = (uint16_t*)OCR_VAL(T, L - 'A'); + PWM_PRINT(*OCRVAL16); + } + SERIAL_ECHOPGM(" WGM: ", WGM); + com_print(T,L); + SERIAL_ECHOPGM(" CS: ", (*TCCRB & (_BV(CS_0) | _BV(CS_1) | _BV(CS_2)) )); + SERIAL_ECHOPGM(" TCCR", AS_DIGIT(T), "A: ", *TCCRA); + SERIAL_ECHOPGM(" TCCR", AS_DIGIT(T), "B: ", *TCCRB); + + const uint8_t *TMSK = (uint8_t*)TIMSK(T); + SERIAL_ECHOPGM(" TIMSK", AS_DIGIT(T), ": ", *TMSK); + + const uint8_t OCIE = L - 'A' + 1; + if (N == 3) { if (WGM == 0 || WGM == 2 || WGM == 4 || WGM == 6) err_is_counter(); } + else { if (WGM == 0 || WGM == 4 || WGM == 12 || WGM == 13) err_is_counter(); } + if (TEST(*TMSK, OCIE)) err_is_interrupt(); + if (TEST(*TMSK, TOIE)) err_prob_interrupt(); +} + +static void pwm_details(uint8_t pin) { + switch (digitalPinToTimer_DEBUG(pin)) { + + #if ABTEST(0) + #ifdef TIMER0A + #if !AVR_AT90USB1286_FAMILY // not available in Teensyduino type IDEs + case TIMER0A: timer_prefix(0, 'A', 3); break; + #endif + #endif + case TIMER0B: timer_prefix(0, 'B', 3); break; + #endif + + #if ABTEST(1) + case TIMER1A: timer_prefix(1, 'A', 4); break; + case TIMER1B: timer_prefix(1, 'B', 4); break; + #if defined(COM1C1) && defined(TIMER1C) + case TIMER1C: timer_prefix(1, 'C', 4); break; + #endif + #endif + + #if ABTEST(2) + case TIMER2A: timer_prefix(2, 'A', 3); break; + case TIMER2B: timer_prefix(2, 'B', 3); break; + #endif + + #if ABTEST(3) + case TIMER3A: timer_prefix(3, 'A', 4); break; + case TIMER3B: timer_prefix(3, 'B', 4); break; + #ifdef COM3C1 + case TIMER3C: timer_prefix(3, 'C', 4); break; + #endif + #endif + + #ifdef TCCR4A + case TIMER4A: timer_prefix(4, 'A', 4); break; + case TIMER4B: timer_prefix(4, 'B', 4); break; + case TIMER4C: timer_prefix(4, 'C', 4); break; + #endif + + #if ABTEST(5) + case TIMER5A: timer_prefix(5, 'A', 4); break; + case TIMER5B: timer_prefix(5, 'B', 4); break; + case TIMER5C: timer_prefix(5, 'C', 4); break; + #endif + + case NOT_ON_TIMER: break; + + } + SERIAL_ECHOPGM(" "); + + // on pins that have two PWMs, print info on second PWM + #if AVR_ATmega2560_FAMILY || AVR_AT90USB1286_FAMILY + // looking for port B7 - PWMs 0A and 1C + if (digitalPinToPort_DEBUG(pin) == 'B' - 64 && 0x80 == digitalPinToBitMask_DEBUG(pin)) { + #if !AVR_AT90USB1286_FAMILY + SERIAL_ECHOPGM("\n ."); + SERIAL_ECHO_SP(18); + SERIAL_ECHOPGM("TIMER1C"); + print_is_also_tied(); + timer_prefix(1, 'C', 4); + #else + SERIAL_ECHOPGM("\n ."); + SERIAL_ECHO_SP(18); + SERIAL_ECHOPGM("TIMER0A"); + print_is_also_tied(); + timer_prefix(0, 'A', 3); + #endif + } + #else + UNUSED(print_is_also_tied); + #endif +} // pwm_details + +#ifndef digitalRead_mod // Use Teensyduino's version of digitalRead - it doesn't disable the PWMs + int digitalRead_mod(const int8_t pin) { // same as digitalRead except the PWM stop section has been removed + const uint8_t port = digitalPinToPort_DEBUG(pin); + return (port != NOT_A_PIN) && (*portInputRegister(port) & digitalPinToBitMask_DEBUG(pin)) ? HIGH : LOW; + } +#endif + +#ifndef PRINT_PORT + + void print_port(int8_t pin) { // print port number + #ifdef digitalPinToPort_DEBUG + uint8_t x; + SERIAL_ECHOPGM(" Port: "); + #if AVR_AT90USB1286_FAMILY + x = (pin == 46 || pin == 47) ? 'E' : digitalPinToPort_DEBUG(pin) + 64; + #else + x = digitalPinToPort_DEBUG(pin) + 64; + #endif + SERIAL_CHAR(x); + + #if AVR_AT90USB1286_FAMILY + if (pin == 46) + x = '2'; + else if (pin == 47) + x = '3'; + else { + uint8_t temp = digitalPinToBitMask_DEBUG(pin); + for (x = '0'; x < '9' && temp != 1; x++) temp >>= 1; + } + #else + uint8_t temp = digitalPinToBitMask_DEBUG(pin); + for (x = '0'; x < '9' && temp != 1; x++) temp >>= 1; + #endif + SERIAL_CHAR(x); + #else + SERIAL_ECHO_SP(10); + #endif + } + + #define PRINT_PORT(p) print_port(p) + +#endif + +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) + +#undef ABTEST diff --git a/src/HAL/AVR/pinsDebug_Teensyduino.h b/src/HAL/AVR/pinsDebug_Teensyduino.h new file mode 100644 index 0000000..582ae79 --- /dev/null +++ b/src/HAL/AVR/pinsDebug_Teensyduino.h @@ -0,0 +1,111 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// +// some of the pin mapping functions of the Teensduino extension to the Arduino IDE +// do not function the same as the other Arduino extensions +// + + +#define TEENSYDUINO_IDE + +//digitalPinToTimer(pin) function works like Arduino but Timers are not defined +#define TIMER0B 1 +#define TIMER1A 7 +#define TIMER1B 8 +#define TIMER1C 9 +#define TIMER2A 6 +#define TIMER2B 2 +#define TIMER3A 5 +#define TIMER3B 4 +#define TIMER3C 3 + +// digitalPinToPort function just returns the pin number so need to create our own +#define PA 1 +#define PB 2 +#define PC 3 +#define PD 4 +#define PE 5 +#define PF 6 + +#undef digitalPinToPort + +const uint8_t PROGMEM digital_pin_to_port_PGM[] = { + PD, // 0 - PD0 - INT0 - PWM + PD, // 1 - PD1 - INT1 - PWM + PD, // 2 - PD2 - INT2 - RX + PD, // 3 - PD3 - INT3 - TX + PD, // 4 - PD4 + PD, // 5 - PD5 + PD, // 6 - PD6 + PD, // 7 - PD7 + PE, // 8 - PE0 + PE, // 9 - PE1 + PC, // 10 - PC0 + PC, // 11 - PC1 + PC, // 12 - PC2 + PC, // 13 - PC3 + PC, // 14 - PC4 - PWM + PC, // 15 - PC5 - PWM + PC, // 16 - PC6 - PWM + PC, // 17 - PC7 + PE, // 18 - PE6 - INT6 + PE, // 19 - PE7 - INT7 + PB, // 20 - PB0 + PB, // 21 - PB1 + PB, // 22 - PB2 + PB, // 23 - PB3 + PB, // 24 - PB4 - PWM + PB, // 25 - PB5 - PWM + PB, // 26 - PB6 - PWM + PB, // 27 - PB7 - PWM + PA, // 28 - PA0 + PA, // 29 - PA1 + PA, // 30 - PA2 + PA, // 31 - PA3 + PA, // 32 - PA4 + PA, // 33 - PA5 + PA, // 34 - PA6 + PA, // 35 - PA7 + PE, // 36 - PE4 - INT4 + PE, // 37 - PE5 - INT5 + PF, // 38 - PF0 - A0 + PF, // 39 - PF1 - A1 + PF, // 40 - PF2 - A2 + PF, // 41 - PF3 - A3 + PF, // 42 - PF4 - A4 + PF, // 43 - PF5 - A5 + PF, // 44 - PF6 - A6 + PF, // 45 - PF7 - A7 + PE, // 46 - PE2 (not defined in teensyduino) + PE, // 47 - PE3 (not defined in teensyduino) +}; + +#define digitalPinToPort(P) ( pgm_read_byte( digital_pin_to_port_PGM + (P) ) ) + +// digitalPinToBitMask(pin) is OK + +#define digitalRead_mod(p) extDigitalRead(p) // Teensyduino's version of digitalRead doesn't + // disable the PWMs so we can use it as is + +// portModeRegister(pin) is OK diff --git a/src/HAL/AVR/pinsDebug_plus_70.h b/src/HAL/AVR/pinsDebug_plus_70.h new file mode 100644 index 0000000..d9aa44c --- /dev/null +++ b/src/HAL/AVR/pinsDebug_plus_70.h @@ -0,0 +1,332 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Structures for 2560 family boards that use more than 70 pins + */ + +#if MB(BQ_ZUM_MEGA_3D, MINIRAMBO, SCOOVO_X9H, TRIGORILLA_14) + #undef NUM_DIGITAL_PINS + #define NUM_DIGITAL_PINS 85 +#elif MB(MIGHTYBOARD_REVE) + #undef NUM_DIGITAL_PINS + #define NUM_DIGITAL_PINS 80 +#endif + +#define PA 1 +#define PB 2 +#define PC 3 +#define PD 4 +#define PE 5 +#define PF 6 +#define PG 7 +#define PH 8 +#define PJ 10 +#define PK 11 +#define PL 12 + +const uint8_t PROGMEM digital_pin_to_port_PGM_plus_70[] = { + // PORTLIST + // ------------------------ + PE , // PE 0 ** 0 ** USART0_RX + PE , // PE 1 ** 1 ** USART0_TX + PE , // PE 4 ** 2 ** PWM2 + PE , // PE 5 ** 3 ** PWM3 + PG , // PG 5 ** 4 ** PWM4 + PE , // PE 3 ** 5 ** PWM5 + PH , // PH 3 ** 6 ** PWM6 + PH , // PH 4 ** 7 ** PWM7 + PH , // PH 5 ** 8 ** PWM8 + PH , // PH 6 ** 9 ** PWM9 + PB , // PB 4 ** 10 ** PWM10 + PB , // PB 5 ** 11 ** PWM11 + PB , // PB 6 ** 12 ** PWM12 + PB , // PB 7 ** 13 ** PWM13 + PJ , // PJ 1 ** 14 ** USART3_TX + PJ , // PJ 0 ** 15 ** USART3_RX + PH , // PH 1 ** 16 ** USART2_TX + PH , // PH 0 ** 17 ** USART2_RX + PD , // PD 3 ** 18 ** USART1_TX + PD , // PD 2 ** 19 ** USART1_RX + PD , // PD 1 ** 20 ** I2C_SDA + PD , // PD 0 ** 21 ** I2C_SCL + PA , // PA 0 ** 22 ** D22 + PA , // PA 1 ** 23 ** D23 + PA , // PA 2 ** 24 ** D24 + PA , // PA 3 ** 25 ** D25 + PA , // PA 4 ** 26 ** D26 + PA , // PA 5 ** 27 ** D27 + PA , // PA 6 ** 28 ** D28 + PA , // PA 7 ** 29 ** D29 + PC , // PC 7 ** 30 ** D30 + PC , // PC 6 ** 31 ** D31 + PC , // PC 5 ** 32 ** D32 + PC , // PC 4 ** 33 ** D33 + PC , // PC 3 ** 34 ** D34 + PC , // PC 2 ** 35 ** D35 + PC , // PC 1 ** 36 ** D36 + PC , // PC 0 ** 37 ** D37 + PD , // PD 7 ** 38 ** D38 + PG , // PG 2 ** 39 ** D39 + PG , // PG 1 ** 40 ** D40 + PG , // PG 0 ** 41 ** D41 + PL , // PL 7 ** 42 ** D42 + PL , // PL 6 ** 43 ** D43 + PL , // PL 5 ** 44 ** D44 + PL , // PL 4 ** 45 ** D45 + PL , // PL 3 ** 46 ** D46 + PL , // PL 2 ** 47 ** D47 + PL , // PL 1 ** 48 ** D48 + PL , // PL 0 ** 49 ** D49 + PB , // PB 3 ** 50 ** SPI_MISO + PB , // PB 2 ** 51 ** SPI_MOSI + PB , // PB 1 ** 52 ** SPI_SCK + PB , // PB 0 ** 53 ** SPI_SS + PF , // PF 0 ** 54 ** A0 + PF , // PF 1 ** 55 ** A1 + PF , // PF 2 ** 56 ** A2 + PF , // PF 3 ** 57 ** A3 + PF , // PF 4 ** 58 ** A4 + PF , // PF 5 ** 59 ** A5 + PF , // PF 6 ** 60 ** A6 + PF , // PF 7 ** 61 ** A7 + PK , // PK 0 ** 62 ** A8 + PK , // PK 1 ** 63 ** A9 + PK , // PK 2 ** 64 ** A10 + PK , // PK 3 ** 65 ** A11 + PK , // PK 4 ** 66 ** A12 + PK , // PK 5 ** 67 ** A13 + PK , // PK 6 ** 68 ** A14 + PK , // PK 7 ** 69 ** A15 + PG , // PG 4 ** 70 ** + PG , // PG 3 ** 71 ** + PJ , // PJ 2 ** 72 ** + PJ , // PJ 3 ** 73 ** + PJ , // PJ 7 ** 74 ** + PJ , // PJ 4 ** 75 ** + PJ , // PJ 5 ** 76 ** + PJ , // PJ 6 ** 77 ** + PE , // PE 2 ** 78 ** + PE , // PE 6 ** 79 ** + PE , // PE 7 ** 80 ** + PD , // PD 4 ** 81 ** + PD , // PD 5 ** 82 ** + PD , // PD 6 ** 83 ** + PH , // PH 2 ** 84 ** + PH , // PH 7 ** 85 ** +}; + +#define digitalPinToPort_plus_70(P) ( pgm_read_byte( digital_pin_to_port_PGM_plus_70 + (P) ) ) + +const uint8_t PROGMEM digital_pin_to_bit_mask_PGM_plus_70[] = { + // PIN IN PORT + // ------------------------ + _BV( 0 ) , // PE 0 ** 0 ** USART0_RX + _BV( 1 ) , // PE 1 ** 1 ** USART0_TX + _BV( 4 ) , // PE 4 ** 2 ** PWM2 + _BV( 5 ) , // PE 5 ** 3 ** PWM3 + _BV( 5 ) , // PG 5 ** 4 ** PWM4 + _BV( 3 ) , // PE 3 ** 5 ** PWM5 + _BV( 3 ) , // PH 3 ** 6 ** PWM6 + _BV( 4 ) , // PH 4 ** 7 ** PWM7 + _BV( 5 ) , // PH 5 ** 8 ** PWM8 + _BV( 6 ) , // PH 6 ** 9 ** PWM9 + _BV( 4 ) , // PB 4 ** 10 ** PWM10 + _BV( 5 ) , // PB 5 ** 11 ** PWM11 + _BV( 6 ) , // PB 6 ** 12 ** PWM12 + _BV( 7 ) , // PB 7 ** 13 ** PWM13 + _BV( 1 ) , // PJ 1 ** 14 ** USART3_TX + _BV( 0 ) , // PJ 0 ** 15 ** USART3_RX + _BV( 1 ) , // PH 1 ** 16 ** USART2_TX + _BV( 0 ) , // PH 0 ** 17 ** USART2_RX + _BV( 3 ) , // PD 3 ** 18 ** USART1_TX + _BV( 2 ) , // PD 2 ** 19 ** USART1_RX + _BV( 1 ) , // PD 1 ** 20 ** I2C_SDA + _BV( 0 ) , // PD 0 ** 21 ** I2C_SCL + _BV( 0 ) , // PA 0 ** 22 ** D22 + _BV( 1 ) , // PA 1 ** 23 ** D23 + _BV( 2 ) , // PA 2 ** 24 ** D24 + _BV( 3 ) , // PA 3 ** 25 ** D25 + _BV( 4 ) , // PA 4 ** 26 ** D26 + _BV( 5 ) , // PA 5 ** 27 ** D27 + _BV( 6 ) , // PA 6 ** 28 ** D28 + _BV( 7 ) , // PA 7 ** 29 ** D29 + _BV( 7 ) , // PC 7 ** 30 ** D30 + _BV( 6 ) , // PC 6 ** 31 ** D31 + _BV( 5 ) , // PC 5 ** 32 ** D32 + _BV( 4 ) , // PC 4 ** 33 ** D33 + _BV( 3 ) , // PC 3 ** 34 ** D34 + _BV( 2 ) , // PC 2 ** 35 ** D35 + _BV( 1 ) , // PC 1 ** 36 ** D36 + _BV( 0 ) , // PC 0 ** 37 ** D37 + _BV( 7 ) , // PD 7 ** 38 ** D38 + _BV( 2 ) , // PG 2 ** 39 ** D39 + _BV( 1 ) , // PG 1 ** 40 ** D40 + _BV( 0 ) , // PG 0 ** 41 ** D41 + _BV( 7 ) , // PL 7 ** 42 ** D42 + _BV( 6 ) , // PL 6 ** 43 ** D43 + _BV( 5 ) , // PL 5 ** 44 ** D44 + _BV( 4 ) , // PL 4 ** 45 ** D45 + _BV( 3 ) , // PL 3 ** 46 ** D46 + _BV( 2 ) , // PL 2 ** 47 ** D47 + _BV( 1 ) , // PL 1 ** 48 ** D48 + _BV( 0 ) , // PL 0 ** 49 ** D49 + _BV( 3 ) , // PB 3 ** 50 ** SPI_MISO + _BV( 2 ) , // PB 2 ** 51 ** SPI_MOSI + _BV( 1 ) , // PB 1 ** 52 ** SPI_SCK + _BV( 0 ) , // PB 0 ** 53 ** SPI_SS + _BV( 0 ) , // PF 0 ** 54 ** A0 + _BV( 1 ) , // PF 1 ** 55 ** A1 + _BV( 2 ) , // PF 2 ** 56 ** A2 + _BV( 3 ) , // PF 3 ** 57 ** A3 + _BV( 4 ) , // PF 4 ** 58 ** A4 + _BV( 5 ) , // PF 5 ** 59 ** A5 + _BV( 6 ) , // PF 6 ** 60 ** A6 + _BV( 7 ) , // PF 7 ** 61 ** A7 + _BV( 0 ) , // PK 0 ** 62 ** A8 + _BV( 1 ) , // PK 1 ** 63 ** A9 + _BV( 2 ) , // PK 2 ** 64 ** A10 + _BV( 3 ) , // PK 3 ** 65 ** A11 + _BV( 4 ) , // PK 4 ** 66 ** A12 + _BV( 5 ) , // PK 5 ** 67 ** A13 + _BV( 6 ) , // PK 6 ** 68 ** A14 + _BV( 7 ) , // PK 7 ** 69 ** A15 + _BV( 4 ) , // PG 4 ** 70 ** + _BV( 3 ) , // PG 3 ** 71 ** + _BV( 2 ) , // PJ 2 ** 72 ** + _BV( 3 ) , // PJ 3 ** 73 ** + _BV( 7 ) , // PJ 7 ** 74 ** + _BV( 4 ) , // PJ 4 ** 75 ** + _BV( 5 ) , // PJ 5 ** 76 ** + _BV( 6 ) , // PJ 6 ** 77 ** + _BV( 2 ) , // PE 2 ** 78 ** + _BV( 6 ) , // PE 6 ** 79 ** + _BV( 7 ) , // PE 7 ** 80 ** + _BV( 4 ) , // PD 4 ** 81 ** + _BV( 5 ) , // PD 5 ** 82 ** + _BV( 6 ) , // PD 6 ** 83 ** + _BV( 2 ) , // PH 2 ** 84 ** + _BV( 7 ) , // PH 7 ** 85 ** +}; + +#define digitalPinToBitMask_plus_70(P) ( pgm_read_byte( digital_pin_to_bit_mask_PGM_plus_70 + (P) ) ) + + +const uint8_t PROGMEM digital_pin_to_timer_PGM_plus_70[] = { + // TIMERS + // ------------------------ + NOT_ON_TIMER , // PE 0 ** 0 ** USART0_RX + NOT_ON_TIMER , // PE 1 ** 1 ** USART0_TX + TIMER3B , // PE 4 ** 2 ** PWM2 + TIMER3C , // PE 5 ** 3 ** PWM3 + TIMER0B , // PG 5 ** 4 ** PWM4 + TIMER3A , // PE 3 ** 5 ** PWM5 + TIMER4A , // PH 3 ** 6 ** PWM6 + TIMER4B , // PH 4 ** 7 ** PWM7 + TIMER4C , // PH 5 ** 8 ** PWM8 + TIMER2B , // PH 6 ** 9 ** PWM9 + TIMER2A , // PB 4 ** 10 ** PWM10 + TIMER1A , // PB 5 ** 11 ** PWM11 + TIMER1B , // PB 6 ** 12 ** PWM12 + TIMER0A , // PB 7 ** 13 ** PWM13 + NOT_ON_TIMER , // PJ 1 ** 14 ** USART3_TX + NOT_ON_TIMER , // PJ 0 ** 15 ** USART3_RX + NOT_ON_TIMER , // PH 1 ** 16 ** USART2_TX + NOT_ON_TIMER , // PH 0 ** 17 ** USART2_RX + NOT_ON_TIMER , // PD 3 ** 18 ** USART1_TX + NOT_ON_TIMER , // PD 2 ** 19 ** USART1_RX + NOT_ON_TIMER , // PD 1 ** 20 ** I2C_SDA + NOT_ON_TIMER , // PD 0 ** 21 ** I2C_SCL + NOT_ON_TIMER , // PA 0 ** 22 ** D22 + NOT_ON_TIMER , // PA 1 ** 23 ** D23 + NOT_ON_TIMER , // PA 2 ** 24 ** D24 + NOT_ON_TIMER , // PA 3 ** 25 ** D25 + NOT_ON_TIMER , // PA 4 ** 26 ** D26 + NOT_ON_TIMER , // PA 5 ** 27 ** D27 + NOT_ON_TIMER , // PA 6 ** 28 ** D28 + NOT_ON_TIMER , // PA 7 ** 29 ** D29 + NOT_ON_TIMER , // PC 7 ** 30 ** D30 + NOT_ON_TIMER , // PC 6 ** 31 ** D31 + NOT_ON_TIMER , // PC 5 ** 32 ** D32 + NOT_ON_TIMER , // PC 4 ** 33 ** D33 + NOT_ON_TIMER , // PC 3 ** 34 ** D34 + NOT_ON_TIMER , // PC 2 ** 35 ** D35 + NOT_ON_TIMER , // PC 1 ** 36 ** D36 + NOT_ON_TIMER , // PC 0 ** 37 ** D37 + NOT_ON_TIMER , // PD 7 ** 38 ** D38 + NOT_ON_TIMER , // PG 2 ** 39 ** D39 + NOT_ON_TIMER , // PG 1 ** 40 ** D40 + NOT_ON_TIMER , // PG 0 ** 41 ** D41 + NOT_ON_TIMER , // PL 7 ** 42 ** D42 + NOT_ON_TIMER , // PL 6 ** 43 ** D43 + TIMER5C , // PL 5 ** 44 ** D44 + TIMER5B , // PL 4 ** 45 ** D45 + TIMER5A , // PL 3 ** 46 ** D46 + NOT_ON_TIMER , // PL 2 ** 47 ** D47 + NOT_ON_TIMER , // PL 1 ** 48 ** D48 + NOT_ON_TIMER , // PL 0 ** 49 ** D49 + NOT_ON_TIMER , // PB 3 ** 50 ** SPI_MISO + NOT_ON_TIMER , // PB 2 ** 51 ** SPI_MOSI + NOT_ON_TIMER , // PB 1 ** 52 ** SPI_SCK + NOT_ON_TIMER , // PB 0 ** 53 ** SPI_SS + NOT_ON_TIMER , // PF 0 ** 54 ** A0 + NOT_ON_TIMER , // PF 1 ** 55 ** A1 + NOT_ON_TIMER , // PF 2 ** 56 ** A2 + NOT_ON_TIMER , // PF 3 ** 57 ** A3 + NOT_ON_TIMER , // PF 4 ** 58 ** A4 + NOT_ON_TIMER , // PF 5 ** 59 ** A5 + NOT_ON_TIMER , // PF 6 ** 60 ** A6 + NOT_ON_TIMER , // PF 7 ** 61 ** A7 + NOT_ON_TIMER , // PK 0 ** 62 ** A8 + NOT_ON_TIMER , // PK 1 ** 63 ** A9 + NOT_ON_TIMER , // PK 2 ** 64 ** A10 + NOT_ON_TIMER , // PK 3 ** 65 ** A11 + NOT_ON_TIMER , // PK 4 ** 66 ** A12 + NOT_ON_TIMER , // PK 5 ** 67 ** A13 + NOT_ON_TIMER , // PK 6 ** 68 ** A14 + NOT_ON_TIMER , // PK 7 ** 69 ** A15 + NOT_ON_TIMER , // PG 4 ** 70 ** + NOT_ON_TIMER , // PG 3 ** 71 ** + NOT_ON_TIMER , // PJ 2 ** 72 ** + NOT_ON_TIMER , // PJ 3 ** 73 ** + NOT_ON_TIMER , // PJ 7 ** 74 ** + NOT_ON_TIMER , // PJ 4 ** 75 ** + NOT_ON_TIMER , // PJ 5 ** 76 ** + NOT_ON_TIMER , // PJ 6 ** 77 ** + NOT_ON_TIMER , // PE 2 ** 78 ** + NOT_ON_TIMER , // PE 6 ** 79 ** +}; + +#define digitalPinToTimer_plus_70(P) ( pgm_read_byte( digital_pin_to_timer_PGM_plus_70 + (P) ) ) + +/** + * Interrupts that are not implemented + * + * INT6 E6 79 + * INT7 E7 80 + * PCINT11 J2 72 + * PCINT12 J3 73 + * PCINT13 J4 75 + * PCINT14 J5 76 + * PCINT15 J6 77 + */ diff --git a/src/HAL/AVR/spi_pins.h b/src/HAL/AVR/spi_pins.h new file mode 100644 index 0000000..8319729 --- /dev/null +++ b/src/HAL/AVR/spi_pins.h @@ -0,0 +1,65 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Define SPI Pins: SCK, MISO, MOSI, SS + */ +#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) + #define AVR_SCK_PIN 13 + #define AVR_MISO_PIN 12 + #define AVR_MOSI_PIN 11 + #define AVR_SS_PIN 10 +#elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__) + #define AVR_SCK_PIN 7 + #define AVR_MISO_PIN 6 + #define AVR_MOSI_PIN 5 + #define AVR_SS_PIN 4 +#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + #define AVR_SCK_PIN 52 + #define AVR_MISO_PIN 50 + #define AVR_MOSI_PIN 51 + #define AVR_SS_PIN 53 +#elif defined(__AVR_AT90USB1287__) || defined(__AVR_AT90USB1286__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB647__) + #define AVR_SCK_PIN 21 + #define AVR_MISO_PIN 23 + #define AVR_MOSI_PIN 22 + #define AVR_SS_PIN 20 +#elif defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__) + #define AVR_SCK_PIN 10 + #define AVR_MISO_PIN 12 + #define AVR_MOSI_PIN 11 + #define AVR_SS_PIN 16 +#endif + +#ifndef SD_SCK_PIN + #define SD_SCK_PIN AVR_SCK_PIN +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN AVR_MISO_PIN +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN AVR_MOSI_PIN +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN AVR_SS_PIN +#endif diff --git a/src/HAL/AVR/timers.h b/src/HAL/AVR/timers.h new file mode 100644 index 0000000..33c3880 --- /dev/null +++ b/src/HAL/AVR/timers.h @@ -0,0 +1,260 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +// ------------------------ +// Types +// ------------------------ + +typedef uint16_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFF + +// ------------------------ +// Defines +// ------------------------ + +#define HAL_TIMER_RATE ((F_CPU) / 8) // i.e., 2MHz or 2.5MHz + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 1 +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 0 +#endif + +#define TEMP_TIMER_FREQUENCY ((F_CPU) / 64.0 / 256.0) + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE +#define STEPPER_TIMER_PRESCALE 8 +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // Cannot be of type double + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A) +#define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A) + +#define ENABLE_TEMPERATURE_INTERRUPT() SBI(TIMSK0, OCIE0A) +#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0A) +#define TEMPERATURE_ISR_ENABLED() TEST(TIMSK0, OCIE0A) + +FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t) { + switch (timer_num) { + case MF_TIMER_STEP: + // waveform generation = 0100 = CTC + SET_WGM(1, CTC_OCRnA); + + // output mode = 00 (disconnected) + SET_COMA(1, NORMAL); + + // Set the timer pre-scaler + // Generally we use a divider of 8, resulting in a 2MHz timer + // frequency on a 16MHz MCU. If you are going to change this, be + // sure to regenerate speed_lookuptable.h with + // create_speed_lookuptable.py + SET_CS(1, PRESCALER_8); // CS 2 = 1/8 prescaler + + // Init Stepper ISR to 122 Hz for quick starting + // (F_CPU) / (STEPPER_TIMER_PRESCALE) / frequency + OCR1A = 0x4000; + TCNT1 = 0; + break; + + case MF_TIMER_TEMP: + // Use timer0 for temperature measurement + // Interleave temperature interrupt with millies interrupt + OCR0A = 128; + break; + } +} + +#define TIMER_OCR_1 OCR1A +#define TIMER_COUNTER_1 TCNT1 + +#define TIMER_OCR_0 OCR0A +#define TIMER_COUNTER_0 TCNT0 + +#define _CAT(a,V...) a##V +#define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare) +#define HAL_timer_get_compare(timer) _CAT(TIMER_OCR_, timer) +#define HAL_timer_get_count(timer) _CAT(TIMER_COUNTER_, timer) + +/** + * On AVR there is no hardware prioritization and preemption of + * interrupts, so this emulates it. The UART has first priority + * (otherwise, characters will be lost due to UART overflow). + * Then: Stepper, Endstops, Temperature, and -finally- all others. + */ +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP + +#ifndef HAL_STEP_TIMER_ISR + +/* 18 cycles maximum latency */ +#define HAL_STEP_TIMER_ISR() \ +extern "C" void TIMER1_COMPA_vect() __attribute__ ((signal, naked, used, externally_visible)); \ +extern "C" void TIMER1_COMPA_vect_bottom() asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \ +void TIMER1_COMPA_vect() { \ + __asm__ __volatile__ ( \ + A("push r16") /* 2 Save R16 */ \ + A("in r16, __SREG__") /* 1 Get SREG */ \ + A("push r16") /* 2 Save SREG into stack */ \ + A("lds r16, %[timsk0]") /* 2 Load into R0 the Temperature timer Interrupt mask register */ \ + A("push r16") /* 2 Save TIMSK0 into the stack */ \ + A("andi r16,~%[msk0]") /* 1 Disable the temperature ISR */ \ + A("sts %[timsk0], r16") /* 2 And set the new value */ \ + A("lds r16, %[timsk1]") /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \ + A("andi r16,~%[msk1]") /* 1 Disable the stepper ISR */ \ + A("sts %[timsk1], r16") /* 2 And set the new value */ \ + A("push r16") /* 2 Save TIMSK1 into stack */ \ + A("in r16, 0x3B") /* 1 Get RAMPZ register */ \ + A("push r16") /* 2 Save RAMPZ into stack */ \ + A("in r16, 0x3C") /* 1 Get EIND register */ \ + A("push r0") /* C runtime can modify all the following registers without restoring them */ \ + A("push r1") \ + A("push r18") \ + A("push r19") \ + A("push r20") \ + A("push r21") \ + A("push r22") \ + A("push r23") \ + A("push r24") \ + A("push r25") \ + A("push r26") \ + A("push r27") \ + A("push r30") \ + A("push r31") \ + A("clr r1") /* C runtime expects this register to be 0 */ \ + A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */ \ + A("pop r31") \ + A("pop r30") \ + A("pop r27") \ + A("pop r26") \ + A("pop r25") \ + A("pop r24") \ + A("pop r23") \ + A("pop r22") \ + A("pop r21") \ + A("pop r20") \ + A("pop r19") \ + A("pop r18") \ + A("pop r1") \ + A("pop r0") \ + A("out 0x3C, r16") /* 1 Restore EIND register */ \ + A("pop r16") /* 2 Get the original RAMPZ register value */ \ + A("out 0x3B, r16") /* 1 Restore RAMPZ register to its original value */ \ + A("pop r16") /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \ + A("ori r16,%[msk1]") /* 1 Reenable the stepper ISR */ \ + A("cli") /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \ + A("sts %[timsk1], r16") /* 2 And restore the old value - This reenables the stepper ISR */ \ + A("pop r16") /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \ + A("sts %[timsk0], r16") /* 2 And restore the old value - This reenables the temperature ISR */ \ + A("pop r16") /* 2 Get the old SREG value */ \ + A("out __SREG__, r16") /* 1 And restore the SREG value */ \ + A("pop r16") /* 2 Restore R16 value */ \ + A("reti") /* 4 Return from interrupt */ \ + : \ + : [timsk0] "i" ((uint16_t)&TIMSK0), \ + [timsk1] "i" ((uint16_t)&TIMSK1), \ + [msk0] "M" ((uint8_t)(1<. + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#if defined(ARDUINO) && !defined(ARDUINO_ARCH_STM32) && !defined(ARDUINO_ARCH_SAM) + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include "../shared/Marduino.h" +#include "../shared/Delay.h" + +#include + +static uint8_t u8g_bitData, u8g_bitNotData, u8g_bitClock, u8g_bitNotClock; +static volatile uint8_t *u8g_outData, *u8g_outClock; + +static void u8g_com_arduino_init_shift_out(uint8_t dataPin, uint8_t clockPin) { + u8g_outData = portOutputRegister(digitalPinToPort(dataPin)); + u8g_outClock = portOutputRegister(digitalPinToPort(clockPin)); + u8g_bitData = digitalPinToBitMask(dataPin); + u8g_bitClock = digitalPinToBitMask(clockPin); + + u8g_bitNotClock = u8g_bitClock; + u8g_bitNotClock ^= 0xFF; + + u8g_bitNotData = u8g_bitData; + u8g_bitNotData ^= 0xFF; +} + +void u8g_spiSend_sw_AVR_mode_0(uint8_t val) { + uint8_t bitData = u8g_bitData, + bitNotData = u8g_bitNotData, + bitClock = u8g_bitClock, + bitNotClock = u8g_bitNotClock; + volatile uint8_t *outData = u8g_outData, + *outClock = u8g_outClock; + U8G_ATOMIC_START(); + LOOP_L_N(i, 8) { + if (val & 0x80) + *outData |= bitData; + else + *outData &= bitNotData; + *outClock |= bitClock; + val <<= 1; + *outClock &= bitNotClock; + } + U8G_ATOMIC_END(); +} + +void u8g_spiSend_sw_AVR_mode_3(uint8_t val) { + uint8_t bitData = u8g_bitData, + bitNotData = u8g_bitNotData, + bitClock = u8g_bitClock, + bitNotClock = u8g_bitNotClock; + volatile uint8_t *outData = u8g_outData, + *outClock = u8g_outClock; + U8G_ATOMIC_START(); + LOOP_L_N(i, 8) { + *outClock &= bitNotClock; + if (val & 0x80) + *outData |= bitData; + else + *outData &= bitNotData; + *outClock |= bitClock; + val <<= 1; + } + U8G_ATOMIC_END(); +} + + +#if ENABLED(FYSETC_MINI_12864) + #define SPISEND_SW_AVR u8g_spiSend_sw_AVR_mode_3 +#else + #define SPISEND_SW_AVR u8g_spiSend_sw_AVR_mode_0 +#endif + +uint8_t u8g_com_HAL_AVR_sw_sp_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + u8g_com_arduino_init_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK]); + u8g_com_arduino_assign_pin_output_high(u8g); + u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 0); + u8g_com_arduino_digital_write(u8g, U8G_PI_MOSI, 0); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_com_arduino_digital_write(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if ENABLED(FYSETC_MINI_12864) // LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 1); // Set SCK to mode 3 idle state before CS goes active + u8g_com_arduino_digital_write(u8g, U8G_PI_CS, LOW); + } + else { + u8g_com_arduino_digital_write(u8g, U8G_PI_CS, HIGH); + u8g_com_arduino_digital_write(u8g, U8G_PI_SCK, 0); // Set SCK to mode 0 idle state after CS goes inactive + } + #else + u8g_com_arduino_digital_write(u8g, U8G_PI_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + SPISEND_SW_AVR(arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + SPISEND_SW_AVR(*ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + SPISEND_SW_AVR(u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_com_arduino_digital_write(u8g, U8G_PI_A0, arg_val); + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/HAL.cpp b/src/HAL/DUE/HAL.cpp new file mode 100644 index 0000000..4353f16 --- /dev/null +++ b/src/HAL/DUE/HAL.cpp @@ -0,0 +1,201 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for Arduino Due and compatible (SAM3X8E) + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" +#include "../../MarlinCore.h" + +#include +#include "usb/usb_task.h" + +// ------------------------ +// Public Variables +// ------------------------ + +uint16_t MarlinHAL::adc_result; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(POSTMORTEM_DEBUGGING) + extern void install_min_serial(); +#endif + +void MarlinHAL::init() { + #if ENABLED(SDSUPPORT) + OUT_WRITE(SDSS, HIGH); // Try to set SDSS inactive before any other SPI users start up + #endif + usb_task_init(); // Initialize the USB stack + TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler +} + +void MarlinHAL::init_board() { + #ifdef BOARD_INIT + BOARD_INIT(); + #endif +} + +void MarlinHAL::idletask() { usb_task_idle(); } // Perform USB stack housekeeping + +uint8_t MarlinHAL::get_reset_source() { + switch ((RSTC->RSTC_SR >> 8) & 0x07) { + case 0: return RST_POWER_ON; + case 1: return RST_BACKUP; + case 2: return RST_WATCHDOG; + case 3: return RST_SOFTWARE; + case 4: return RST_EXTERNAL; + default: return 0; + } +} + +void MarlinHAL::reboot() { rstc_start_software_reset(RSTC); } + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + // Initialize watchdog - On SAM3X, Watchdog was already configured + // and enabled or disabled at startup, so no need to reconfigure it + // here. + void MarlinHAL::watchdog_init() { WDT_Restart(WDT); } // Reset watchdog to start clean + + // Reset watchdog. MUST be called at least every 4 seconds after the + // first watchdog_init or AVR will go into emergency procedures. + void MarlinHAL::watchdog_refresh() { watchdogReset(); } + +#endif + +// Override Arduino runtime to either config or disable the watchdog +// +// We need to configure the watchdog as soon as possible in the boot +// process, because watchdog initialization at hardware reset on SAM3X8E +// is unreliable, and there is risk of unintended resets if we delay +// that initialization to a later time. +void watchdogSetup() { + + #if ENABLED(USE_WATCHDOG) + + // 4 seconds timeout + uint32_t timeout = TERN(WATCHDOG_DURATION_8S, 8000, 4000); + + // Calculate timeout value in WDT counter ticks: This assumes + // the slow clock is running at 32.768 kHz watchdog + // frequency is therefore 32768 / 128 = 256 Hz + timeout = (timeout << 8) / 1000; + if (timeout == 0) + timeout = 1; + else if (timeout > 0xFFF) + timeout = 0xFFF; + + // We want to enable the watchdog with the specified timeout + uint32_t value = + WDT_MR_WDV(timeout) | // With the specified timeout + WDT_MR_WDD(timeout) | // and no invalid write window + #if !(SAMV70 || SAMV71 || SAME70 || SAMS70) + WDT_MR_WDRPROC | // WDT fault resets processor only - We want + // to keep PIO controller state + #endif + WDT_MR_WDDBGHLT | // WDT stops in debug state. + WDT_MR_WDIDLEHLT; // WDT stops in idle state. + + #if ENABLED(WATCHDOG_RESET_MANUAL) + // We enable the watchdog timer, but only for the interrupt. + + // Configure WDT to only trigger an interrupt + value |= WDT_MR_WDFIEN; // Enable WDT fault interrupt. + + // Disable WDT interrupt (just in case, to avoid triggering it!) + NVIC_DisableIRQ(WDT_IRQn); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + // Initialize WDT with the given parameters + WDT_Enable(WDT, value); + + // Configure and enable WDT interrupt. + NVIC_ClearPendingIRQ(WDT_IRQn); + NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups + NVIC_EnableIRQ(WDT_IRQn); + + #else + + // a WDT fault triggers a reset + value |= WDT_MR_WDRSTEN; + + // Initialize WDT with the given parameters + WDT_Enable(WDT, value); + + #endif + + // Reset the watchdog + WDT_Restart(WDT); + + #else + + // Make sure to completely disable the Watchdog + WDT_Disable(WDT); + + #endif +} + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +extern "C" { + extern unsigned int _ebss; // end of bss section +} + +// Return free memory between end of heap (or end bss) and whatever is current +int freeMemory() { + int free_memory, heap_end = (int)_sbrk(0); + return (int)&free_memory - (heap_end ?: (int)&_ebss); +} + +// ------------------------ +// Serial Ports +// ------------------------ + +// Forward the default serial ports +#if USING_HW_SERIAL0 + DefaultSerial1 MSerial0(false, Serial); +#endif +#if USING_HW_SERIAL1 + DefaultSerial2 MSerial1(false, Serial1); +#endif +#if USING_HW_SERIAL2 + DefaultSerial3 MSerial2(false, Serial2); +#endif +#if USING_HW_SERIAL3 + DefaultSerial4 MSerial3(false, Serial3); +#endif + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/HAL.h b/src/HAL/DUE/HAL.h new file mode 100644 index 0000000..4d3f482 --- /dev/null +++ b/src/HAL/DUE/HAL.h @@ -0,0 +1,233 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Arduino Due and compatible (SAM3X8E) + */ + +#define CPU_32_BIT + +#include "../shared/Marduino.h" +#include "../shared/eeprom_if.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" + +#include + +#include "../../core/serial_hook.h" + +// ------------------------ +// Serial ports +// ------------------------ + +typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; +typedef ForwardSerial1Class< decltype(Serial1) > DefaultSerial2; +typedef ForwardSerial1Class< decltype(Serial2) > DefaultSerial3; +typedef ForwardSerial1Class< decltype(Serial3) > DefaultSerial4; +extern DefaultSerial1 MSerial0; +extern DefaultSerial2 MSerial1; +extern DefaultSerial3 MSerial2; +extern DefaultSerial4 MSerial3; + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if SERIAL_PORT == -1 || ENABLED(EMERGENCY_PARSER) + #define MYSERIAL1 customizedSerial1 +#elif WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "The required SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." +#endif + +#ifdef SERIAL_PORT_2 + #if SERIAL_PORT_2 == -1 || ENABLED(EMERGENCY_PARSER) + #define MYSERIAL2 customizedSerial2 + #elif WITHIN(SERIAL_PORT_2, 0, 3) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 3, or -1 for USB Serial." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 || ENABLED(EMERGENCY_PARSER) + #define MYSERIAL3 customizedSerial3 + #elif WITHIN(SERIAL_PORT_3, 0, 3) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #else + #error "SERIAL_PORT_3 must be from 0 to 3, or -1 for USB Serial." + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if WITHIN(MMU2_SERIAL_PORT, 0, 3) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #error "MMU2_SERIAL_PORT must be from 0 to 3." + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if WITHIN(LCD_SERIAL_PORT, 0, 3) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #error "LCD_SERIAL_PORT must be from 0 to 3." + #endif +#endif + +#include "MarlinSerial.h" +#include "MarlinSerialUSB.h" + +// ------------------------ +// Types +// ------------------------ + +typedef int8_t pin_t; + +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; + +// +// Interrupts +// +#define sei() interrupts() +#define cli() noInterrupts() + +#define CRITICAL_SECTION_START() const bool _irqon = hal.isr_state(); hal.isr_off() +#define CRITICAL_SECTION_END() if (_irqon) hal.isr_on() + +// +// ADC +// +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 + +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +// +// Tone +// +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); +void noTone(const pin_t _pin); + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +#pragma GCC diagnostic pop + +#ifdef __cplusplus + extern "C" { +#endif +char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s); +#ifdef __cplusplus + } +#endif + +// Return free RAM between end of heap (or end bss) and whatever is current +int freeMemory(); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init(); // Called early in setup() + static void init_board(); // Called less early in setup() + static void reboot(); // Restart the firmware + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) {} + + // Begin ADC sampling on the given channel. Called from Temperature::isr! + static void adc_start(const uint8_t ch) { adc_result = analogRead(ch); } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * No inverting the duty cycle in this HAL. + * No changing the maximum size of the provided value to enable finer PWM duty control in this HAL. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/src/HAL/DUE/HAL_SPI.cpp b/src/HAL/DUE/HAL_SPI.cpp new file mode 100644 index 0000000..7e3fe01 --- /dev/null +++ b/src/HAL/DUE/HAL_SPI.cpp @@ -0,0 +1,819 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Software SPI functions originally from Arduino Sd2Card Library + * Copyright (c) 2009 by William Greiman + * + * Completely rewritten and tuned by Eduardo José Tagle in 2017/2018 + * in ARM thumb2 inline assembler and tuned for maximum speed and performance + * allowing SPI clocks of up to 12 Mhz to increase SD card read/write performance + */ + +/** + * HAL for Arduino Due and compatible (SAM3X8E) + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" +#include "../shared/Delay.h" + +// ------------------------ +// Public functions +// ------------------------ + +#if EITHER(DUE_SOFTWARE_SPI, FORCE_SOFT_SPI) + + // ------------------------ + // Software SPI + // ------------------------ + + // Make sure GCC optimizes this file. + // Note that this line triggers a bug in GCC which is fixed by casting. + // See the note below. + #pragma GCC optimize (3) + + typedef uint8_t (*pfnSpiTransfer)(uint8_t b); + typedef void (*pfnSpiRxBlock)(uint8_t *buf, uint32_t nbyte); + typedef void (*pfnSpiTxBlock)(const uint8_t *buf, uint32_t nbyte); + + /* ---------------- Macros to be able to access definitions from asm */ + #define _PORT(IO) DIO ## IO ## _WPORT + #define _PIN_MASK(IO) MASK(DIO ## IO ## _PIN) + #define _PIN_SHIFT(IO) DIO ## IO ## _PIN + #define PORT(IO) _PORT(IO) + #define PIN_MASK(IO) _PIN_MASK(IO) + #define PIN_SHIFT(IO) _PIN_SHIFT(IO) + + // run at ~8 .. ~10Mhz - Tx version (Rx data discarded) + static uint8_t spiTransferTx0(uint8_t bout) { // using Mode 0 + uint32_t MOSI_PORT_PLUS30 = ((uint32_t) PORT(SD_MOSI_PIN)) + 0x30; /* SODR of port */ + uint32_t MOSI_MASK = PIN_MASK(SD_MOSI_PIN); + uint32_t SCK_PORT_PLUS30 = ((uint32_t) PORT(SD_SCK_PIN)) + 0x30; /* SODR of port */ + uint32_t SCK_MASK = PIN_MASK(SD_SCK_PIN); + uint32_t idx = 0; + + /* Negate bout, as the assembler requires a negated value */ + bout = ~bout; + + /* The software SPI routine */ + __asm__ __volatile__( + A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax + + /* Bit 7 */ + A("ubfx %[idx],%[txval],#7,#1") /* Place bit 7 in bit 0 of idx*/ + + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#6,#1") /* Place bit 6 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 6 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#5,#1") /* Place bit 5 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 5 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#4,#1") /* Place bit 4 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 4 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#3,#1") /* Place bit 3 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 3 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#2,#1") /* Place bit 2 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 2 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#1,#1") /* Place bit 1 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 1 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[idx],%[txval],#0,#1") /* Place bit 0 in bit 0 of idx*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 0 */ + A("str %[mosi_mask],[%[mosi_port], %[idx],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("nop") /* Result will be 0 */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + : [idx]"+r"( idx ) + : [txval]"r"( bout ) , + [mosi_mask]"r"( MOSI_MASK ), + [mosi_port]"r"( MOSI_PORT_PLUS30 ), + [sck_mask]"r"( SCK_MASK ), + [sck_port]"r"( SCK_PORT_PLUS30 ) + : "cc" + ); + + return 0; + } + + // Calculates the bit band alias address and returns a pointer address to word. + // addr: The byte address of bitbanding bit. + // bit: The bit position of bitbanding bit. + #define BITBAND_ADDRESS(addr, bit) \ + (((uint32_t)(addr) & 0xF0000000) + 0x02000000 + ((uint32_t)(addr)&0xFFFFF)*32 + (bit)*4) + + // run at ~8 .. ~10Mhz - Rx version (Tx line not altered) + static uint8_t spiTransferRx0(uint8_t) { // using Mode 0 + uint32_t bin = 0; + uint32_t work = 0; + uint32_t BITBAND_MISO_PORT = BITBAND_ADDRESS( ((uint32_t)PORT(SD_MISO_PIN))+0x3C, PIN_SHIFT(SD_MISO_PIN)); /* PDSR of port in bitband area */ + uint32_t SCK_PORT_PLUS30 = ((uint32_t) PORT(SD_SCK_PIN)) + 0x30; /* SODR of port */ + uint32_t SCK_MASK = PIN_MASK(SD_SCK_PIN); + + /* The software SPI routine */ + __asm__ __volatile__( + A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax + + /* bit 7 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#7,#1") /* Store read bit as the bit 7 */ + + /* bit 6 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#6,#1") /* Store read bit as the bit 6 */ + + /* bit 5 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#5,#1") /* Store read bit as the bit 5 */ + + /* bit 4 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#4,#1") /* Store read bit as the bit 4 */ + + /* bit 3 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#3,#1") /* Store read bit as the bit 3 */ + + /* bit 2 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#2,#1") /* Store read bit as the bit 2 */ + + /* bit 1 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#1,#1") /* Store read bit as the bit 1 */ + + /* bit 0 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#0,#1") /* Store read bit as the bit 0 */ + + : [bin]"+r"(bin), + [work]"+r"(work) + : [bitband_miso_port]"r"( BITBAND_MISO_PORT ), + [sck_mask]"r"( SCK_MASK ), + [sck_port]"r"( SCK_PORT_PLUS30 ) + : "cc" + ); + + return bin; + } + + // run at ~4Mhz + static uint8_t spiTransfer1(uint8_t b) { // using Mode 0 + int bits = 8; + do { + WRITE(SD_MOSI_PIN, b & 0x80); + b <<= 1; // little setup time + + WRITE(SD_SCK_PIN, HIGH); + DELAY_NS(125); // 10 cycles @ 84mhz + + b |= (READ(SD_MISO_PIN) != 0); + + WRITE(SD_SCK_PIN, LOW); + DELAY_NS(125); // 10 cycles @ 84mhz + } while (--bits); + return b; + } + + // all the others + static uint16_t spiDelayNS = 4000; // 4000ns => 125khz + + static uint8_t spiTransferX(uint8_t b) { // using Mode 0 + int bits = 8; + do { + WRITE(SD_MOSI_PIN, b & 0x80); + b <<= 1; // little setup time + + WRITE(SD_SCK_PIN, HIGH); + DELAY_NS(spiDelayNS); + + b |= (READ(SD_MISO_PIN) != 0); + + WRITE(SD_SCK_PIN, LOW); + DELAY_NS(spiDelayNS); + } while (--bits); + return b; + } + + // Pointers to generic functions for byte transfers + + /** + * Note: The cast is unnecessary, but without it, this file triggers a GCC 4.8.3-2014 bug. + * Later GCC versions do not have this problem, but at this time (May 2018) Arduino still + * uses that buggy and obsolete GCC version!! + */ + static pfnSpiTransfer spiTransferRx = (pfnSpiTransfer)spiTransferX; + static pfnSpiTransfer spiTransferTx = (pfnSpiTransfer)spiTransferX; + + // Block transfers run at ~8 .. ~10Mhz - Tx version (Rx data discarded) + static void spiTxBlock0(const uint8_t *ptr, uint32_t todo) { + uint32_t MOSI_PORT_PLUS30 = ((uint32_t) PORT(SD_MOSI_PIN)) + 0x30; /* SODR of port */ + uint32_t MOSI_MASK = PIN_MASK(SD_MOSI_PIN); + uint32_t SCK_PORT_PLUS30 = ((uint32_t) PORT(SD_SCK_PIN)) + 0x30; /* SODR of port */ + uint32_t SCK_MASK = PIN_MASK(SD_SCK_PIN); + uint32_t work = 0; + uint32_t txval = 0; + + /* The software SPI routine */ + __asm__ __volatile__( + A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax + + L("loop%=") + A("ldrb.w %[txval], [%[ptr]], #1") /* Load value to send, increment buffer */ + A("mvn %[txval],%[txval]") /* Negate value */ + + /* Bit 7 */ + A("ubfx %[work],%[txval],#7,#1") /* Place bit 7 in bit 0 of work*/ + + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#6,#1") /* Place bit 6 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 6 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#5,#1") /* Place bit 5 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 5 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#4,#1") /* Place bit 4 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 4 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#3,#1") /* Place bit 3 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 3 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#2,#1") /* Place bit 2 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 2 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#1,#1") /* Place bit 1 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 1 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ubfx %[work],%[txval],#0,#1") /* Place bit 0 in bit 0 of work*/ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + + /* Bit 0 */ + A("str %[mosi_mask],[%[mosi_port], %[work],LSL #2]") /* Access the proper SODR or CODR registers based on that bit */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("subs %[todo],#1") /* Decrement count of pending words to send, update status */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bne.n loop%=") /* Repeat until done */ + + : [ptr]"+r" ( ptr ) , + [todo]"+r" ( todo ) , + [work]"+r"( work ) , + [txval]"+r"( txval ) + : [mosi_mask]"r"( MOSI_MASK ), + [mosi_port]"r"( MOSI_PORT_PLUS30 ), + [sck_mask]"r"( SCK_MASK ), + [sck_port]"r"( SCK_PORT_PLUS30 ) + : "cc" + ); + } + + static void spiRxBlock0(uint8_t *ptr, uint32_t todo) { + uint32_t bin = 0; + uint32_t work = 0; + uint32_t BITBAND_MISO_PORT = BITBAND_ADDRESS( ((uint32_t)PORT(SD_MISO_PIN))+0x3C, PIN_SHIFT(SD_MISO_PIN)); /* PDSR of port in bitband area */ + uint32_t SCK_PORT_PLUS30 = ((uint32_t) PORT(SD_SCK_PIN)) + 0x30; /* SODR of port */ + uint32_t SCK_MASK = PIN_MASK(SD_SCK_PIN); + + /* The software SPI routine */ + __asm__ __volatile__( + A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax + + L("loop%=") + + /* bit 7 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#7,#1") /* Store read bit as the bit 7 */ + + /* bit 6 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#6,#1") /* Store read bit as the bit 6 */ + + /* bit 5 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#5,#1") /* Store read bit as the bit 5 */ + + /* bit 4 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#4,#1") /* Store read bit as the bit 4 */ + + /* bit 3 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#3,#1") /* Store read bit as the bit 3 */ + + /* bit 2 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#2,#1") /* Store read bit as the bit 2 */ + + /* bit 1 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#1,#1") /* Store read bit as the bit 1 */ + + /* bit 0 */ + A("str %[sck_mask],[%[sck_port]]") /* SODR */ + A("ldr %[work],[%[bitband_miso_port]]") /* PDSR on bitband area for required bit: work will be 1 or 0 based on port */ + A("str %[sck_mask],[%[sck_port],#0x4]") /* CODR */ + A("bfi %[bin],%[work],#0,#1") /* Store read bit as the bit 0 */ + + A("subs %[todo],#1") /* Decrement count of pending words to send, update status */ + A("strb.w %[bin], [%[ptr]], #1") /* Store read value into buffer, increment buffer pointer */ + A("bne.n loop%=") /* Repeat until done */ + + : [ptr]"+r"(ptr), + [todo]"+r"(todo), + [bin]"+r"(bin), + [work]"+r"(work) + : [bitband_miso_port]"r"( BITBAND_MISO_PORT ), + [sck_mask]"r"( SCK_MASK ), + [sck_port]"r"( SCK_PORT_PLUS30 ) + : "cc" + ); + } + + static void spiTxBlockX(const uint8_t *buf, uint32_t todo) { + do { + (void)spiTransferTx(*buf++); + } while (--todo); + } + + static void spiRxBlockX(uint8_t *buf, uint32_t todo) { + do { + *buf++ = spiTransferRx(0xFF); + } while (--todo); + } + + // Pointers to generic functions for block transfers + static pfnSpiTxBlock spiTxBlock = (pfnSpiTxBlock)spiTxBlockX; + static pfnSpiRxBlock spiRxBlock = (pfnSpiRxBlock)spiRxBlockX; + + #if MB(ALLIGATOR) + #define _SS_WRITE(S) WRITE(SD_SS_PIN, S) + #else + #define _SS_WRITE(S) NOOP + #endif + + void spiBegin() { + SET_OUTPUT(SD_SS_PIN); + _SS_WRITE(HIGH); + SET_OUTPUT(SD_SCK_PIN); + SET_INPUT(SD_MISO_PIN); + SET_OUTPUT(SD_MOSI_PIN); + } + + uint8_t spiRec() { + _SS_WRITE(LOW); + WRITE(SD_MOSI_PIN, HIGH); // Output 1s 1 + uint8_t b = spiTransferRx(0xFF); + _SS_WRITE(HIGH); + return b; + } + + void spiRead(uint8_t *buf, uint16_t nbyte) { + if (nbyte) { + _SS_WRITE(LOW); + WRITE(SD_MOSI_PIN, HIGH); // Output 1s 1 + spiRxBlock(buf, nbyte); + _SS_WRITE(HIGH); + } + } + + void spiSend(uint8_t b) { + _SS_WRITE(LOW); + (void)spiTransferTx(b); + _SS_WRITE(HIGH); + } + + void spiSendBlock(uint8_t token, const uint8_t *buf) { + _SS_WRITE(LOW); + (void)spiTransferTx(token); + spiTxBlock(buf, 512); + _SS_WRITE(HIGH); + } + + /** + * spiRate should be + * 0 : 8 - 10 MHz + * 1 : 4 - 5 MHz + * 2 : 2 - 2.5 MHz + * 3 : 1 - 1.25 MHz + * 4 : 500 - 625 kHz + * 5 : 250 - 312 kHz + * 6 : 125 - 156 kHz + */ + void spiInit(uint8_t spiRate) { + switch (spiRate) { + case 0: + spiTransferTx = (pfnSpiTransfer)spiTransferTx0; + spiTransferRx = (pfnSpiTransfer)spiTransferRx0; + spiTxBlock = (pfnSpiTxBlock)spiTxBlock0; + spiRxBlock = (pfnSpiRxBlock)spiRxBlock0; + break; + case 1: + spiTransferTx = (pfnSpiTransfer)spiTransfer1; + spiTransferRx = (pfnSpiTransfer)spiTransfer1; + spiTxBlock = (pfnSpiTxBlock)spiTxBlockX; + spiRxBlock = (pfnSpiRxBlock)spiRxBlockX; + break; + default: + spiDelayNS = 4000 >> (6 - spiRate); // spiRate of 2 gives the maximum error with current CPU + spiTransferTx = (pfnSpiTransfer)spiTransferX; + spiTransferRx = (pfnSpiTransfer)spiTransferX; + spiTxBlock = (pfnSpiTxBlock)spiTxBlockX; + spiRxBlock = (pfnSpiRxBlock)spiRxBlockX; + break; + } + + _SS_WRITE(HIGH); + WRITE(SD_MOSI_PIN, HIGH); + WRITE(SD_SCK_PIN, LOW); + } + + /** Begin SPI transaction, set clock, bit order, data mode */ + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + // TODO: to be implemented + } + + #pragma GCC reset_options + +#else // !SOFTWARE_SPI + + #define WHILE_TX(N) while ((SPI0->SPI_SR & SPI_SR_TDRE) == (N)) + #define WHILE_RX(N) while ((SPI0->SPI_SR & SPI_SR_RDRF) == (N)) + #define FLUSH_TX() do{ WHILE_RX(1) SPI0->SPI_RDR; }while(0) + + #if MB(ALLIGATOR) + + // slave selects controlled by SPI controller + // doesn't support changing SPI speeds for SD card + + // ------------------------ + // hardware SPI + // ------------------------ + static bool spiInitialized = false; + + void spiInit(uint8_t spiRate) { + if (spiInitialized) return; + + // 8.4 MHz, 4 MHz, 2 MHz, 1 MHz, 0.5 MHz, 0.329 MHz, 0.329 MHz + constexpr int spiDivider[] = { 10, 21, 42, 84, 168, 255, 255 }; + if (spiRate > 6) spiRate = 1; + + // Set SPI mode 1, clock, select not active after transfer, with delay between transfers + SPI_ConfigureNPCS(SPI0, SPI_CHAN_DAC, + SPI_CSR_CSAAT | SPI_CSR_SCBR(spiDivider[spiRate]) | + SPI_CSR_DLYBCT(1)); + // Set SPI mode 0, clock, select not active after transfer, with delay between transfers + SPI_ConfigureNPCS(SPI0, SPI_CHAN_EEPROM1, SPI_CSR_NCPHA | + SPI_CSR_CSAAT | SPI_CSR_SCBR(spiDivider[spiRate]) | + SPI_CSR_DLYBCT(1)); + + // Set SPI mode 0, clock, select not active after transfer, with delay between transfers + SPI_ConfigureNPCS(SPI0, SPI_CHAN, SPI_CSR_NCPHA | + SPI_CSR_CSAAT | SPI_CSR_SCBR(spiDivider[spiRate]) | + SPI_CSR_DLYBCT(1)); + SPI_Enable(SPI0); + spiInitialized = true; + } + + void spiBegin() { + if (spiInitialized) return; + + // Configure SPI pins + PIO_Configure( + g_APinDescription[SD_SCK_PIN].pPort, + g_APinDescription[SD_SCK_PIN].ulPinType, + g_APinDescription[SD_SCK_PIN].ulPin, + g_APinDescription[SD_SCK_PIN].ulPinConfiguration); + PIO_Configure( + g_APinDescription[SD_MOSI_PIN].pPort, + g_APinDescription[SD_MOSI_PIN].ulPinType, + g_APinDescription[SD_MOSI_PIN].ulPin, + g_APinDescription[SD_MOSI_PIN].ulPinConfiguration); + PIO_Configure( + g_APinDescription[SD_MISO_PIN].pPort, + g_APinDescription[SD_MISO_PIN].ulPinType, + g_APinDescription[SD_MISO_PIN].ulPin, + g_APinDescription[SD_MISO_PIN].ulPinConfiguration); + + // set master mode, peripheral select, fault detection + SPI_Configure(SPI0, ID_SPI0, SPI_MR_MSTR | SPI_MR_MODFDIS | SPI_MR_PS); + SPI_Enable(SPI0); + + SET_OUTPUT(DAC0_SYNC_PIN); + #if HAS_MULTI_EXTRUDER + OUT_WRITE(DAC1_SYNC_PIN, HIGH); + #endif + WRITE(DAC0_SYNC_PIN, HIGH); + OUT_WRITE(SPI_EEPROM1_CS_PIN, HIGH); + OUT_WRITE(SPI_EEPROM2_CS_PIN, HIGH); + OUT_WRITE(SPI_FLASH_CS_PIN, HIGH); + WRITE(SD_SS_PIN, HIGH); + + OUT_WRITE(SDSS, LOW); + + PIO_Configure( + g_APinDescription[SPI_PIN].pPort, + g_APinDescription[SPI_PIN].ulPinType, + g_APinDescription[SPI_PIN].ulPin, + g_APinDescription[SPI_PIN].ulPinConfiguration + ); + + spiInit(1); + } + + // Read single byte from SPI + uint8_t spiRec() { + // write dummy byte with address and end transmission flag + SPI0->SPI_TDR = 0x000000FF | SPI_PCS(SPI_CHAN) | SPI_TDR_LASTXFER; + + WHILE_TX(0); + WHILE_RX(0); + + //DELAY_US(1U); + return SPI0->SPI_RDR; + } + + uint8_t spiRec(uint32_t chan) { + + WHILE_TX(0); + FLUSH_RX(); + + // write dummy byte with address and end transmission flag + SPI0->SPI_TDR = 0x000000FF | SPI_PCS(chan) | SPI_TDR_LASTXFER; + WHILE_RX(0); + + return SPI0->SPI_RDR; + } + + // Read from SPI into buffer + void spiRead(uint8_t *buf, uint16_t nbyte) { + if (!nbyte) return; + --nbyte; + for (int i = 0; i < nbyte; i++) { + //WHILE_TX(0); + SPI0->SPI_TDR = 0x000000FF | SPI_PCS(SPI_CHAN); + WHILE_RX(0); + buf[i] = SPI0->SPI_RDR; + //DELAY_US(1U); + } + buf[nbyte] = spiRec(); + } + + // Write single byte to SPI + void spiSend(const byte b) { + // write byte with address and end transmission flag + SPI0->SPI_TDR = (uint32_t)b | SPI_PCS(SPI_CHAN) | SPI_TDR_LASTXFER; + WHILE_TX(0); + WHILE_RX(0); + SPI0->SPI_RDR; + //DELAY_US(1U); + } + + void spiSend(const uint8_t *buf, size_t nbyte) { + if (!nbyte) return; + --nbyte; + for (size_t i = 0; i < nbyte; i++) { + SPI0->SPI_TDR = (uint32_t)buf[i] | SPI_PCS(SPI_CHAN); + WHILE_TX(0); + WHILE_RX(0); + SPI0->SPI_RDR; + //DELAY_US(1U); + } + spiSend(buf[nbyte]); + } + + void spiSend(uint32_t chan, byte b) { + WHILE_TX(0); + // write byte with address and end transmission flag + SPI0->SPI_TDR = (uint32_t)b | SPI_PCS(chan) | SPI_TDR_LASTXFER; + WHILE_RX(0); + FLUSH_RX(); + } + + void spiSend(uint32_t chan, const uint8_t *buf, size_t nbyte) { + if (!nbyte) return; + --nbyte; + for (size_t i = 0; i < nbyte; i++) { + WHILE_TX(0); + SPI0->SPI_TDR = (uint32_t)buf[i] | SPI_PCS(chan); + WHILE_RX(0); + FLUSH_RX(); + } + spiSend(chan, buf[nbyte]); + } + + // Write from buffer to SPI + void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI0->SPI_TDR = (uint32_t)token | SPI_PCS(SPI_CHAN); + WHILE_TX(0); + //WHILE_RX(0); + //SPI0->SPI_RDR; + for (int i = 0; i < 511; i++) { + SPI0->SPI_TDR = (uint32_t)buf[i] | SPI_PCS(SPI_CHAN); + WHILE_TX(0); + WHILE_RX(0); + SPI0->SPI_RDR; + //DELAY_US(1U); + } + spiSend(buf[511]); + } + + /** Begin SPI transaction, set clock, bit order, data mode */ + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + // TODO: to be implemented + } + + #else // U8G compatible hardware SPI + + #define SPI_MODE_0_DUE_HW 2 // DUE CPHA control bit is inverted + #define SPI_MODE_1_DUE_HW 3 + #define SPI_MODE_2_DUE_HW 0 + #define SPI_MODE_3_DUE_HW 1 + + /** + * The DUE SPI controller is set up so the upper word of the longword + * written to the transmit data register selects which SPI Chip Select + * Register is used. This allows different streams to have different SPI + * settings. + * + * In practice it's spooky. Some combinations hang the system, while others + * upset the peripheral device. + * + * SPI mode should be the same for all streams. The FYSETC_MINI_12864 gets + * upset if the clock phase changes after chip select goes active. + * + * SPI_CSR_CSAAT should be set for all streams. If not the WHILE_TX(0) + * macro returns immediately which can result in the SPI chip select going + * inactive before all the data has been sent. + * + * The TMC2130 library uses SPI0->SPI_CSR[3]. + * + * The U8G hardware SPI uses SPI0->SPI_CSR[0]. The system hangs and/or the + * FYSETC_MINI_12864 gets upset if lower baud rates are used and the SD card + * is inserted or removed. + * + * The SD card uses SPI0->SPI_CSR[3]. Efforts were made to use [1] and [2] + * but they all resulted in hangs or garbage on the LCD. + * + * The SPI controlled chip selects are NOT enabled in the GPIO controller. + * The application must control the chip select. + * + * All of the above can be avoided by defining FORCE_SOFT_SPI to force the + * display to use software SPI. + */ + + void spiInit(uint8_t spiRate=6) { // Default to slowest rate if not specified) + // Also sets U8G SPI rate to 4MHz and the SPI mode to 3 + + // 8.4 MHz, 4 MHz, 2 MHz, 1 MHz, 0.5 MHz, 0.329 MHz, 0.329 MHz + constexpr int spiDivider[] = { 10, 21, 42, 84, 168, 255, 255 }; + if (spiRate > 6) spiRate = 1; + + // Enable PIOA and SPI0 + REG_PMC_PCER0 = (1UL << ID_PIOA) | (1UL << ID_SPI0); + + // Disable PIO on A26 and A27 + REG_PIOA_PDR = 0x0C000000; + OUT_WRITE(SDSS, HIGH); + + // Reset SPI0 (from sam lib) + SPI0->SPI_CR = SPI_CR_SPIDIS; + SPI0->SPI_CR = SPI_CR_SWRST; + SPI0->SPI_CR = SPI_CR_SWRST; + SPI0->SPI_CR = SPI_CR_SPIEN; + + // TMC2103 compatible setup + // Master mode, no fault detection, PCS bits in data written to TDR select CSR register + SPI0->SPI_MR = SPI_MR_MSTR | SPI_MR_PS | SPI_MR_MODFDIS; + // SPI mode 3, 8 Bit data transfer, baud rate + SPI0->SPI_CSR[3] = SPI_CSR_SCBR(spiDivider[spiRate]) | SPI_CSR_CSAAT | SPI_MODE_3_DUE_HW; // use same CSR as TMC2130 + SPI0->SPI_CSR[0] = SPI_CSR_SCBR(spiDivider[1]) | SPI_CSR_CSAAT | SPI_MODE_3_DUE_HW; // U8G default to 4MHz + } + + void spiBegin() { spiInit(); } + + static uint8_t spiTransfer(uint8_t data) { + WHILE_TX(0); + SPI0->SPI_TDR = (uint32_t)data | 0x00070000UL; // Add TMC2130 PCS bits to every byte (use SPI0->SPI_CSR[3]) + WHILE_TX(0); + WHILE_RX(0); + return SPI0->SPI_RDR; + } + + uint8_t spiRec() { return (uint8_t)spiTransfer(0xFF); } + + void spiRead(uint8_t *buf, uint16_t nbyte) { + for (int i = 0; i < nbyte; i++) + buf[i] = spiTransfer(0xFF); + } + + void spiSend(uint8_t data) { spiTransfer(data); } + + void spiSend(const uint8_t *buf, size_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) + spiTransfer(buf[i]); + } + + void spiSendBlock(uint8_t token, const uint8_t *buf) { + spiTransfer(token); + for (uint16_t i = 0; i < 512; i++) + spiTransfer(buf[i]); + } + + #endif // !ALLIGATOR +#endif // !SOFTWARE_SPI + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/InterruptVectors.cpp b/src/HAL/DUE/InterruptVectors.cpp new file mode 100644 index 0000000..e4e0ce9 --- /dev/null +++ b/src/HAL/DUE/InterruptVectors.cpp @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * InterruptVectors_Due.cpp - This module relocates the Interrupt vector table to SRAM, + * allowing to register new interrupt handlers at runtime. Specially valuable and needed + * because Arduino runtime allocates some interrupt handlers that we NEED to override to + * properly support extended functionality, as for example, USB host or USB device (MSD, MTP) + * and custom serial port handlers, and we don't actually want to modify and/or recompile the + * Arduino runtime. We just want to run as much as possible on Stock Arduino + * + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + */ +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" +#include "InterruptVectors.h" + +/* The relocated Exception/Interrupt Table - According to the ARM + reference manual, alignment to 128 bytes should suffice, but in + practice, we need alignment to 256 bytes to make this work in all + cases */ +__attribute__ ((aligned(256))) +static DeviceVectors ram_tab = { nullptr }; + +/** + * This function checks if the exception/interrupt table is already in SRAM or not. + * If it is not, then it copies the ROM table to the SRAM and relocates the table + * by reprogramming the NVIC registers + */ +static pfnISR_Handler* get_relocated_table_addr() { + // Get the address of the interrupt/exception table + uint32_t isrtab = SCB->VTOR; + + // If already relocated, we are done! + if (isrtab >= IRAM0_ADDR) + return (pfnISR_Handler*)isrtab; + + // Get the address of the table stored in FLASH + const pfnISR_Handler* romtab = (const pfnISR_Handler*)isrtab; + + // Copy it to SRAM + memcpy(&ram_tab, romtab, sizeof(ram_tab)); + + // Disable global interrupts + CRITICAL_SECTION_START(); + + // Set the vector table base address to the SRAM copy + SCB->VTOR = (uint32_t)(&ram_tab); + + // Reenable interrupts + CRITICAL_SECTION_END(); + + // Return the address of the table + return (pfnISR_Handler*)(&ram_tab); +} + +pfnISR_Handler install_isr(IRQn_Type irq, pfnISR_Handler newHandler) { + // Get the address of the relocated table + pfnISR_Handler *isrtab = get_relocated_table_addr(); + + // Disable global interrupts + CRITICAL_SECTION_START(); + + // Get the original handler + pfnISR_Handler oldHandler = isrtab[irq + 16]; + + // Install the new one + isrtab[irq + 16] = newHandler; + + // Reenable interrupts + CRITICAL_SECTION_END(); + + // Return the original one + return oldHandler; +} + +#endif diff --git a/src/HAL/DUE/InterruptVectors.h b/src/HAL/DUE/InterruptVectors.h new file mode 100644 index 0000000..6faeb34 --- /dev/null +++ b/src/HAL/DUE/InterruptVectors.h @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * InterruptVectors_Due.h + * + * This module relocates the Interrupt vector table to SRAM, allowing new + * interrupt handlers to be added at runtime. This is required because the + * Arduino runtime steals interrupt handlers that Marlin MUST use to support + * extended functionality such as USB hosts and USB devices (MSD, MTP) and + * custom serial port handlers. Rather than modifying and/or recompiling the + * Arduino runtime, We just want to run as much as possible on Stock Arduino. + * + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + */ + +#ifdef ARDUINO_ARCH_SAM + +// ISR handler type +typedef void (*pfnISR_Handler)(); + +// Install a new interrupt vector handler for the given irq, returning the old one +pfnISR_Handler install_isr(IRQn_Type irq, pfnISR_Handler newHandler); + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/MarlinSPI.h b/src/HAL/DUE/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/DUE/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/DUE/MarlinSerial.cpp b/src/HAL/DUE/MarlinSerial.cpp new file mode 100644 index 0000000..638f7a1 --- /dev/null +++ b/src/HAL/DUE/MarlinSerial.cpp @@ -0,0 +1,494 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * MarlinSerial_Due.cpp - Hardware serial library for Arduino DUE + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + * Based on MarlinSerial for AVR, copyright (c) 2006 Nicholas Zambetti. All right reserved. + */ +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" + +#include "MarlinSerial.h" +#include "InterruptVectors.h" +#include "../../MarlinCore.h" + +template typename MarlinSerial::ring_buffer_r MarlinSerial::rx_buffer = { 0, 0, { 0 } }; +template typename MarlinSerial::ring_buffer_t MarlinSerial::tx_buffer = { 0 }; +template bool MarlinSerial::_written = false; +template uint8_t MarlinSerial::xon_xoff_state = MarlinSerial::XON_XOFF_CHAR_SENT | MarlinSerial::XON_CHAR; +template uint8_t MarlinSerial::rx_dropped_bytes = 0; +template uint8_t MarlinSerial::rx_buffer_overruns = 0; +template uint8_t MarlinSerial::rx_framing_errors = 0; +template typename MarlinSerial::ring_buffer_pos_t MarlinSerial::rx_max_enqueued = 0; + +// A SW memory barrier, to ensure GCC does not overoptimize loops +#define sw_barrier() asm volatile("": : :"memory"); + +#include "../../feature/e_parser.h" + +// (called with RX interrupts disabled) +template +FORCE_INLINE void MarlinSerial::store_rxd_char() { + + static EmergencyParser::State emergency_state; // = EP_RESET + + // Get the tail - Nothing can alter its value while we are at this ISR + const ring_buffer_pos_t t = rx_buffer.tail; + + // Get the head pointer + ring_buffer_pos_t h = rx_buffer.head; + + // Get the next element + ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Read the character from the USART + uint8_t c = HWUART->UART_RHR; + + if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the RX FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + // Calculate count of bytes stored into the RX buffer + + // Keep track of the maximum count of enqueued bytes + if (Cfg::MAX_RX_QUEUED) NOLESS(rx_max_enqueued, rx_count); + + if (Cfg::XONOFF) { + // If the last char that was sent was an XON + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) { + + // Bytes stored into the RX buffer + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // If over 12.5% of RX buffer capacity, send XOFF before running out of + // RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react + // and stop sending bytes. This translates to 13mS propagation time. + if (rx_count >= (Cfg::RX_SIZE) / 8) { + + // At this point, definitely no TX interrupt was executing, since the TX isr can't be preempted. + // Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens + // to be in the middle of trying to disable the RX interrupt in the main program, eventually the + // enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure + // the sending of the XOFF char is to send it HERE AND NOW. + + // About to send the XOFF char + xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT; + + // Wait until the TX register becomes empty and send it - Here there could be a problem + // - While waiting for the TX register to empty, the RX register could receive a new + // character. This must also handle that situation! + uint32_t status; + while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) { + + if (status & UART_SR_RXRDY) { + // We received a char while waiting for the TX buffer to be empty - Receive and process it! + + i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Read the character from the USART + c = HWUART->UART_RHR; + + if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + } + sw_barrier(); + } + + HWUART->UART_THR = XOFF_CHAR; + + // At this point there could be a race condition between the write() function + // and this sending of the XOFF char. This interrupt could happen between the + // wait to be empty TX buffer loop and the actual write of the character. Since + // the TX buffer is full because it's sending the XOFF char, the only way to be + // sure the write() function will succeed is to wait for the XOFF char to be + // completely sent. Since an extra character could be received during the wait + // it must also be handled! + while (!((status = HWUART->UART_SR) & UART_SR_TXRDY)) { + + if (status & UART_SR_RXRDY) { + // A char arrived while waiting for the TX buffer to be empty - Receive and process it! + + i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + + // Read the character from the USART + c = HWUART->UART_RHR; + + if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c); + + // If the character is to be stored at the index just before the tail + // (such that the head would advance to the current tail), the FIFO is + // full, so don't write the character or advance the head. + if (i != t) { + rx_buffer.buffer[h] = c; + h = i; + } + else if (Cfg::DROPPED_RX && !++rx_dropped_bytes) + --rx_dropped_bytes; + } + sw_barrier(); + } + + // At this point everything is ready. The write() function won't + // have any issues writing to the UART TX register if it needs to! + } + } + } + + // Store the new head value + rx_buffer.head = h; +} + +template +FORCE_INLINE void MarlinSerial::_tx_thr_empty_irq() { + if (Cfg::TX_SIZE > 0) { + // Read positions + uint8_t t = tx_buffer.tail; + const uint8_t h = tx_buffer.head; + + if (Cfg::XONOFF) { + // If an XON char is pending to be sent, do it now + if (xon_xoff_state == XON_CHAR) { + + // Send the character + HWUART->UART_THR = XON_CHAR; + + // Remember we sent it. + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + + // If nothing else to transmit, just disable TX interrupts. + if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY; + + return; + } + } + + // If nothing to transmit, just disable TX interrupts. This could + // happen as the result of the non atomicity of the disabling of RX + // interrupts that could end reenabling TX interrupts as a side effect. + if (h == t) { + HWUART->UART_IDR = UART_IDR_TXRDY; + return; + } + + // There is something to TX, Send the next byte + const uint8_t c = tx_buffer.buffer[t]; + t = (t + 1) & (Cfg::TX_SIZE - 1); + HWUART->UART_THR = c; + tx_buffer.tail = t; + + // Disable interrupts if there is nothing to transmit following this byte + if (h == t) HWUART->UART_IDR = UART_IDR_TXRDY; + } +} + +template +void MarlinSerial::UART_ISR() { + const uint32_t status = HWUART->UART_SR; + + // Data received? + if (status & UART_SR_RXRDY) store_rxd_char(); + + if (Cfg::TX_SIZE > 0) { + // Something to send, and TX interrupts are enabled (meaning something to send)? + if ((status & UART_SR_TXRDY) && (HWUART->UART_IMR & UART_IMR_TXRDY)) _tx_thr_empty_irq(); + } + + // Acknowledge errors + if ((status & UART_SR_OVRE) || (status & UART_SR_FRAME)) { + if (Cfg::DROPPED_RX && (status & UART_SR_OVRE) && !++rx_dropped_bytes) --rx_dropped_bytes; + if (Cfg::RX_OVERRUNS && (status & UART_SR_OVRE) && !++rx_buffer_overruns) --rx_buffer_overruns; + if (Cfg::RX_FRAMING_ERRORS && (status & UART_SR_FRAME) && !++rx_framing_errors) --rx_framing_errors; + + // TODO: error reporting outside ISR + HWUART->UART_CR = UART_CR_RSTSTA; + } +} + +// Public Methods +template +void MarlinSerial::begin(const long baud_setting) { + + // Disable UART interrupt in NVIC + NVIC_DisableIRQ( HWUART_IRQ ); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + // Disable clock + pmc_disable_periph_clk( HWUART_IRQ_ID ); + + // Configure PMC + pmc_enable_periph_clk( HWUART_IRQ_ID ); + + // Disable PDC channel + HWUART->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS; + + // Reset and disable receiver and transmitter + HWUART->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX | UART_CR_RXDIS | UART_CR_TXDIS; + + // Configure mode: 8bit, No parity, 1 bit stop + HWUART->UART_MR = UART_MR_CHMODE_NORMAL | US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | UART_MR_PAR_NO; + + // Configure baudrate (asynchronous, no oversampling) + HWUART->UART_BRGR = (SystemCoreClock / (baud_setting << 4)); + + // Configure interrupts + HWUART->UART_IDR = 0xFFFFFFFF; + HWUART->UART_IER = UART_IER_RXRDY | UART_IER_OVRE | UART_IER_FRAME; + + // Install interrupt handler + install_isr(HWUART_IRQ, UART_ISR); + + // Configure priority. We need a very high priority to avoid losing characters + // and we need to be able to preempt the Stepper ISR and everything else! + // (this could probably be fixed by using DMA with the Serial port) + NVIC_SetPriority(HWUART_IRQ, 1); + + // Enable UART interrupt in NVIC + NVIC_EnableIRQ(HWUART_IRQ); + + // Enable receiver and transmitter + HWUART->UART_CR = UART_CR_RXEN | UART_CR_TXEN; + + if (Cfg::TX_SIZE > 0) _written = false; +} + +template +void MarlinSerial::end() { + // Disable UART interrupt in NVIC + NVIC_DisableIRQ( HWUART_IRQ ); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + pmc_disable_periph_clk( HWUART_IRQ_ID ); +} + +template +int MarlinSerial::peek() { + const int v = rx_buffer.head == rx_buffer.tail ? -1 : rx_buffer.buffer[rx_buffer.tail]; + return v; +} + +template +int MarlinSerial::read() { + + const ring_buffer_pos_t h = rx_buffer.head; + ring_buffer_pos_t t = rx_buffer.tail; + + if (h == t) return -1; + + int v = rx_buffer.buffer[t]; + t = (ring_buffer_pos_t)(t + 1) & (Cfg::RX_SIZE - 1); + + // Advance tail + rx_buffer.tail = t; + + if (Cfg::XONOFF) { + // If the XOFF char was sent, or about to be sent... + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) { + // Get count of bytes in the RX buffer + const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1); + // When below 10% of RX buffer capacity, send XON before running out of RX buffer bytes + if (rx_count < (Cfg::RX_SIZE) / 10) { + if (Cfg::TX_SIZE > 0) { + // Signal we want an XON character to be sent. + xon_xoff_state = XON_CHAR; + // Enable TX isr. + HWUART->UART_IER = UART_IER_TXRDY; + } + else { + // If not using TX interrupts, we must send the XON char now + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier(); + HWUART->UART_THR = XON_CHAR; + } + } + } + } + + return v; +} + +template +typename MarlinSerial::ring_buffer_pos_t MarlinSerial::available() { + const ring_buffer_pos_t h = rx_buffer.head, t = rx_buffer.tail; + return (ring_buffer_pos_t)(Cfg::RX_SIZE + h - t) & (Cfg::RX_SIZE - 1); +} + +template +void MarlinSerial::flush() { + rx_buffer.tail = rx_buffer.head; + + if (Cfg::XONOFF) { + if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XOFF_CHAR) { + if (Cfg::TX_SIZE > 0) { + // Signal we want an XON character to be sent. + xon_xoff_state = XON_CHAR; + // Enable TX isr. + HWUART->UART_IER = UART_IER_TXRDY; + } + else { + // If not using TX interrupts, we must send the XON char now + xon_xoff_state = XON_CHAR | XON_XOFF_CHAR_SENT; + while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier(); + HWUART->UART_THR = XON_CHAR; + } + } + } +} + +template +size_t MarlinSerial::write(const uint8_t c) { + _written = true; + + if (Cfg::TX_SIZE == 0) { + while (!(HWUART->UART_SR & UART_SR_TXRDY)) sw_barrier(); + HWUART->UART_THR = c; + } + else { + + // If the TX interrupts are disabled and the data register + // is empty, just write the byte to the data register and + // be done. This shortcut helps significantly improve the + // effective datarate at high (>500kbit/s) bitrates, where + // interrupt overhead becomes a slowdown. + // Yes, there is a race condition between the sending of the + // XOFF char at the RX isr, but it is properly handled there + if (!(HWUART->UART_IMR & UART_IMR_TXRDY) && (HWUART->UART_SR & UART_SR_TXRDY)) { + HWUART->UART_THR = c; + return 1; + } + + const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1); + + // If global interrupts are disabled (as the result of being called from an ISR)... + if (!hal.isr_state()) { + + // Make room by polling if it is possible to transmit, and do so! + while (i == tx_buffer.tail) { + // If we can transmit another byte, do it. + if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq(); + // Make sure compiler rereads tx_buffer.tail + sw_barrier(); + } + } + else { + // Interrupts are enabled, just wait until there is space + while (i == tx_buffer.tail) sw_barrier(); + } + + // Store new char. head is always safe to move + tx_buffer.buffer[tx_buffer.head] = c; + tx_buffer.head = i; + + // Enable TX isr - Non atomic, but it will eventually enable TX isr + HWUART->UART_IER = UART_IER_TXRDY; + } + return 1; +} + +template +void MarlinSerial::flushTX() { + // TX + + if (Cfg::TX_SIZE == 0) { + // No bytes written, no need to flush. This special case is needed since there's + // no way to force the TXC (transmit complete) bit to 1 during initialization. + if (!_written) return; + + // Wait until everything was transmitted + while (!(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier(); + + // At this point nothing is queued anymore (DRIE is disabled) and + // the hardware finished transmission (TXC is set). + + } + else { + // If we have never written a byte, no need to flush. This special + // case is needed since there is no way to force the TXC (transmit + // complete) bit to 1 during initialization + if (!_written) return; + + // If global interrupts are disabled (as the result of being called from an ISR)... + if (!hal.isr_state()) { + + // Wait until everything was transmitted - We must do polling, as interrupts are disabled + while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) { + // If there is more space, send an extra character + if (HWUART->UART_SR & UART_SR_TXRDY) _tx_thr_empty_irq(); + sw_barrier(); + } + + } + else { + // Wait until everything was transmitted + while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) sw_barrier(); + } + + // At this point nothing is queued anymore (DRIE is disabled) and + // the hardware finished transmission (TXC is set). + } +} + + +// If not using the USB port as serial port +#if defined(SERIAL_PORT) && SERIAL_PORT >= 0 + template class MarlinSerial< MarlinSerialCfg >; + MSerialT1 customizedSerial1(MarlinSerialCfg::EMERGENCYPARSER); +#endif + +#if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0 + template class MarlinSerial< MarlinSerialCfg >; + MSerialT2 customizedSerial2(MarlinSerialCfg::EMERGENCYPARSER); +#endif + +#if defined(SERIAL_PORT_3) && SERIAL_PORT_3 >= 0 + template class MarlinSerial< MarlinSerialCfg >; + MSerialT3 customizedSerial3(MarlinSerialCfg::EMERGENCYPARSER); +#endif + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/MarlinSerial.h b/src/HAL/DUE/MarlinSerial.h new file mode 100644 index 0000000..5a61bff --- /dev/null +++ b/src/HAL/DUE/MarlinSerial.h @@ -0,0 +1,156 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * MarlinSerial_Due.h - Hardware serial library for Arduino DUE + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + * Based on MarlinSerial for AVR, copyright (c) 2006 Nicholas Zambetti. All right reserved. + */ + +#include + +#include "../../inc/MarlinConfigPre.h" +#include "../../core/serial_hook.h" + +// Define constants and variables for buffering incoming serial data. We're +// using a ring buffer (I think), in which rx_buffer_head is the index of the +// location to which to write the next incoming character and rx_buffer_tail +// is the index of the location from which to read. +// 256 is the max limit due to uint8_t head and tail. Use only powers of 2. (...,16,32,64,128,256) +#ifndef RX_BUFFER_SIZE + #define RX_BUFFER_SIZE 128 +#endif +#ifndef TX_BUFFER_SIZE + #define TX_BUFFER_SIZE 32 +#endif + +//#if ENABLED(SERIAL_XON_XOFF) && RX_BUFFER_SIZE < 1024 +// #error "SERIAL_XON_XOFF requires RX_BUFFER_SIZE >= 1024 for reliable transfers without drops." +//#elif RX_BUFFER_SIZE && (RX_BUFFER_SIZE < 2 || !IS_POWER_OF_2(RX_BUFFER_SIZE)) +// #error "RX_BUFFER_SIZE must be a power of 2 greater than 1." +//#elif TX_BUFFER_SIZE && (TX_BUFFER_SIZE < 2 || TX_BUFFER_SIZE > 256 || !IS_POWER_OF_2(TX_BUFFER_SIZE)) +// #error "TX_BUFFER_SIZE must be 0, a power of 2 greater than 1, and no greater than 256." +//#endif + +// Templated type selector +template struct TypeSelector { typedef T type;} ; +template struct TypeSelector { typedef F type; }; + +// Templated structure wrapper +template struct StructWrapper { + constexpr StructWrapper(int) {} + FORCE_INLINE S* operator->() const { return (S*)addr; } +}; + +template +class MarlinSerial { +protected: + // Information for all supported UARTs + static constexpr uint32_t BASES[] = {0x400E0800U, 0x40098000U, 0x4009C000U, 0x400A0000U, 0x400A4000U}; + static constexpr IRQn_Type IRQS[] = { UART_IRQn, USART0_IRQn, USART1_IRQn, USART2_IRQn, USART3_IRQn}; + static constexpr int IRQ_IDS[] = { ID_UART, ID_USART0, ID_USART1, ID_USART2, ID_USART3}; + + // Alias for shorter code + static constexpr StructWrapper HWUART = 0; + static constexpr IRQn_Type HWUART_IRQ = IRQS[Cfg::PORT]; + static constexpr int HWUART_IRQ_ID = IRQ_IDS[Cfg::PORT]; + + // Base size of type on buffer size + typedef typename TypeSelector<(Cfg::RX_SIZE>256), uint16_t, uint8_t>::type ring_buffer_pos_t; + + struct ring_buffer_r { + volatile ring_buffer_pos_t head, tail; + unsigned char buffer[Cfg::RX_SIZE]; + }; + + struct ring_buffer_t { + volatile uint8_t head, tail; + unsigned char buffer[Cfg::TX_SIZE]; + }; + + static ring_buffer_r rx_buffer; + static ring_buffer_t tx_buffer; + static bool _written; + + static constexpr uint8_t XON_XOFF_CHAR_SENT = 0x80, // XON / XOFF Character was sent + XON_XOFF_CHAR_MASK = 0x1F; // XON / XOFF character to send + + // XON / XOFF character definitions + static constexpr uint8_t XON_CHAR = 17, XOFF_CHAR = 19; + static uint8_t xon_xoff_state, + rx_dropped_bytes, + rx_buffer_overruns, + rx_framing_errors; + static ring_buffer_pos_t rx_max_enqueued; + + FORCE_INLINE static void store_rxd_char(); + FORCE_INLINE static void _tx_thr_empty_irq(); + static void UART_ISR(); + +public: + MarlinSerial() {}; + static void begin(const long); + static void end(); + static int peek(); + static int read(); + static void flush(); + static ring_buffer_pos_t available(); + static size_t write(const uint8_t c); + static void flushTX(); + + static bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } + + FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; } + FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; } + FORCE_INLINE static uint8_t framing_errors() { return Cfg::RX_FRAMING_ERRORS ? rx_framing_errors : 0; } + FORCE_INLINE static ring_buffer_pos_t rxMaxEnqueued() { return Cfg::MAX_RX_QUEUED ? rx_max_enqueued : 0; } +}; + +// Serial port configuration +template +struct MarlinSerialCfg { + static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = RX_BUFFER_SIZE; + static constexpr unsigned int TX_SIZE = TX_BUFFER_SIZE; + static constexpr bool XONOFF = ENABLED(SERIAL_XON_XOFF); + static constexpr bool EMERGENCYPARSER = ENABLED(EMERGENCY_PARSER); + static constexpr bool DROPPED_RX = ENABLED(SERIAL_STATS_DROPPED_RX); + static constexpr bool RX_OVERRUNS = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS); + static constexpr bool RX_FRAMING_ERRORS = ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS); + static constexpr bool MAX_RX_QUEUED = ENABLED(SERIAL_STATS_MAX_RX_QUEUED); +}; + +#if defined(SERIAL_PORT) && SERIAL_PORT >= 0 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT1; + extern MSerialT1 customizedSerial1; +#endif + +#if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT2; + extern MSerialT2 customizedSerial2; +#endif + +#if defined(SERIAL_PORT_3) && SERIAL_PORT_3 >= 0 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT3; + extern MSerialT3 customizedSerial3; +#endif diff --git a/src/HAL/DUE/MarlinSerialUSB.cpp b/src/HAL/DUE/MarlinSerialUSB.cpp new file mode 100644 index 0000000..8de2dc7 --- /dev/null +++ b/src/HAL/DUE/MarlinSerialUSB.cpp @@ -0,0 +1,142 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_SAM + +/** + * MarlinSerial_Due.cpp - Hardware serial library for Arduino DUE + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + * Based on MarlinSerial for AVR, copyright (c) 2006 Nicholas Zambetti. All right reserved. + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_USB_SERIAL + +#include "MarlinSerialUSB.h" + +// Imports from Atmel USB Stack/CDC implementation +extern "C" { + bool usb_task_cdc_isenabled(); + bool usb_task_cdc_dtr_active(); + bool udi_cdc_is_rx_ready(); + int udi_cdc_getc(); + bool udi_cdc_is_tx_ready(); + int udi_cdc_putc(int value); +} + +// Pending character +static int pending_char = -1; + +// Public Methods +void MarlinSerialUSB::begin(const long) {} + +void MarlinSerialUSB::end() {} + +int MarlinSerialUSB::peek() { + if (pending_char >= 0) + return pending_char; + + // If USB CDC not enumerated or not configured on the PC side + if (!usb_task_cdc_isenabled()) + return -1; + + // If no bytes sent from the PC + if (!udi_cdc_is_rx_ready()) + return -1; + + pending_char = udi_cdc_getc(); + + TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)pending_char)); + + return pending_char; +} + +int MarlinSerialUSB::read() { + if (pending_char >= 0) { + int ret = pending_char; + pending_char = -1; + return ret; + } + + // If USB CDC not enumerated or not configured on the PC side + if (!usb_task_cdc_isenabled()) + return -1; + + // If no bytes sent from the PC + if (!udi_cdc_is_rx_ready()) + return -1; + + int c = udi_cdc_getc(); + + TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)c)); + + return c; +} + +int MarlinSerialUSB::available() { + if (pending_char > 0) return pending_char; + return pending_char == 0 || + // or USB CDC enumerated and configured on the PC side and some bytes where sent to us */ + (usb_task_cdc_isenabled() && udi_cdc_is_rx_ready()); +} + +void MarlinSerialUSB::flush() { } + +size_t MarlinSerialUSB::write(const uint8_t c) { + + /* Do not even bother sending anything if USB CDC is not enumerated + or not configured on the PC side or there is no program on the PC + listening to our messages */ + if (!usb_task_cdc_isenabled() || !usb_task_cdc_dtr_active()) + return 0; + + /* Wait until the PC has read the pending to be sent data */ + while (usb_task_cdc_isenabled() && + usb_task_cdc_dtr_active() && + !udi_cdc_is_tx_ready()) { + }; + + /* Do not even bother sending anything if USB CDC is not enumerated + or not configured on the PC side or there is no program on the PC + listening to our messages at this point */ + if (!usb_task_cdc_isenabled() || !usb_task_cdc_dtr_active()) + return 0; + + // Fifo full + // udi_cdc_signal_overrun(); + udi_cdc_putc(c); + return 1; +} + +// Preinstantiate +#if SERIAL_PORT == -1 + MSerialT1 customizedSerial1(TERN0(EMERGENCY_PARSER, true)); +#endif +#if SERIAL_PORT_2 == -1 + MSerialT2 customizedSerial2(TERN0(EMERGENCY_PARSER, true)); +#endif +#if SERIAL_PORT_3 == -1 + MSerialT3 customizedSerial3(TERN0(EMERGENCY_PARSER, true)); +#endif + +#endif // HAS_USB_SERIAL +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/MarlinSerialUSB.h b/src/HAL/DUE/MarlinSerialUSB.h new file mode 100644 index 0000000..6da1ef8 --- /dev/null +++ b/src/HAL/DUE/MarlinSerialUSB.h @@ -0,0 +1,65 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * MarlinSerialUSB_Due.h - Hardware Serial over USB (CDC) library for Arduino DUE + * Copyright (c) 2017 Eduardo José Tagle. All right reserved + */ + +#include "../../inc/MarlinConfig.h" +#include "../../core/serial_hook.h" + +#include + +struct MarlinSerialUSB { + void begin(const long); + void end(); + int peek(); + int read(); + void flush(); + int available(); + size_t write(const uint8_t c); + + #if ENABLED(SERIAL_STATS_DROPPED_RX) + FORCE_INLINE uint32_t dropped() { return 0; } + #endif + + #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + FORCE_INLINE int rxMaxEnqueued() { return 0; } + #endif +}; + +#if SERIAL_PORT == -1 + typedef Serial1Class MSerialT1; + extern MSerialT1 customizedSerial1; +#endif + +#if SERIAL_PORT_2 == -1 + typedef Serial1Class MSerialT2; + extern MSerialT2 customizedSerial2; +#endif + +#if SERIAL_PORT_3 == -1 + typedef Serial1Class MSerialT3; + extern MSerialT3 customizedSerial3; +#endif diff --git a/src/HAL/DUE/MinSerial.cpp b/src/HAL/DUE/MinSerial.cpp new file mode 100644 index 0000000..e5b3dbf --- /dev/null +++ b/src/HAL/DUE/MinSerial.cpp @@ -0,0 +1,91 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(POSTMORTEM_DEBUGGING) + +#include "../shared/MinSerial.h" + +#include + +static void TXBegin() { + // Disable UART interrupt in NVIC + NVIC_DisableIRQ( UART_IRQn ); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + // Disable clock + pmc_disable_periph_clk( ID_UART ); + + // Configure PMC + pmc_enable_periph_clk( ID_UART ); + + // Disable PDC channel + UART->UART_PTCR = UART_PTCR_RXTDIS | UART_PTCR_TXTDIS; + + // Reset and disable receiver and transmitter + UART->UART_CR = UART_CR_RSTRX | UART_CR_RSTTX | UART_CR_RXDIS | UART_CR_TXDIS; + + // Configure mode: 8bit, No parity, 1 bit stop + UART->UART_MR = UART_MR_CHMODE_NORMAL | US_MR_CHRL_8_BIT | US_MR_NBSTOP_1_BIT | UART_MR_PAR_NO; + + // Configure baudrate (asynchronous, no oversampling) to BAUDRATE bauds + UART->UART_BRGR = (SystemCoreClock / (BAUDRATE << 4)); + + // Enable receiver and transmitter + UART->UART_CR = UART_CR_RXEN | UART_CR_TXEN; +} + +// A SW memory barrier, to ensure GCC does not overoptimize loops +#define sw_barrier() __asm__ volatile("": : :"memory"); +static void TX(char c) { + while (!(UART->UART_SR & UART_SR_TXRDY)) { WDT_Restart(WDT); sw_barrier(); }; + UART->UART_THR = c; +} + +void install_min_serial() { + HAL_min_serial_init = &TXBegin; + HAL_min_serial_out = &TX; +} + +#if DISABLED(DYNAMIC_VECTORTABLE) +extern "C" { + __attribute__((naked)) void JumpHandler_ASM() { + __asm__ __volatile__ ( + "b CommonHandler_ASM\n" + ); + } + void __attribute__((naked, alias("JumpHandler_ASM"))) HardFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) BusFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) UsageFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) MemManage_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) NMI_Handler(); +} +#endif + +#endif // POSTMORTEM_DEBUGGING +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/Servo.cpp b/src/HAL/DUE/Servo.cpp new file mode 100644 index 0000000..2dab882 --- /dev/null +++ b/src/HAL/DUE/Servo.cpp @@ -0,0 +1,163 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/* + Copyright (c) 2013 Arduino LLC. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library 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 + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "../shared/servo.h" +#include "../shared/servo_private.h" + +static Flags<_Nbr_16timers> DisablePending; // ISR should disable the timer at the next timer reset + +// ------------------------ +/// Interrupt handler for the TC0 channel 1. +// ------------------------ +void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t); + +#ifdef _useTimer1 + void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } +#endif +#ifdef _useTimer2 + void HANDLER_FOR_TIMER2() { Servo_Handler(_timer2, TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); } +#endif +#ifdef _useTimer3 + void HANDLER_FOR_TIMER3() { Servo_Handler(_timer3, TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); } +#endif +#ifdef _useTimer4 + void HANDLER_FOR_TIMER4() { Servo_Handler(_timer4, TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); } +#endif +#ifdef _useTimer5 + void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); } +#endif + +void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { + static int8_t Channel[_Nbr_16timers]; // Servo counters to pulse (or -1 for refresh interval) + int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first + if (cho < 0) { // Channel -1 indicates the refresh interval completed... + tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer + if (DisablePending[timer]) { + // Disabling only after the full servo period expires prevents + // pulses being too close together if immediately re-enabled. + DisablePending.clear(timer); + TC_Stop(tc, channel); + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt + return; + } + } + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW + + Channel[timer] = ++cho; // go to the next channel (or 0) + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { + tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks; + if (SERVO(timer, cho).Pin.isActive) // activated? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH + } + else { + // finished all channels so wait for the refresh period to expire before starting over + const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed + tc->TC_CHANNEL[channel].TC_RA = max(cval, ival); + + Channel[timer] = -1; // reset the timer CCR on the next call + } + + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt +} + +static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { + pmc_enable_periph_clk(id); + TC_Configure(tc, channel, + TC_CMR_WAVE // Waveform mode + | TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC + | (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2 + | (SERVO_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) // MCK/8 + | (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32 + | (SERVO_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) // MCK/128 + ); + + // Wait 1ms before the first ISR + TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms + + // Configure and enable interrupt + NVIC_EnableIRQ(irqn); + tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare + + // Enables the timer clock and performs a software reset to start the counting + TC_Start(tc, channel); +} + +void initISR(const timer16_Sequence_t timer_index) { + CRITICAL_SECTION_START(); + const bool disable_soon = DisablePending[timer_index]; + DisablePending.clear(timer_index); + CRITICAL_SECTION_END(); + + if (!disable_soon) switch (timer_index) { + default: break; + #ifdef _useTimer1 + case _timer1: return _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); + #endif + #ifdef _useTimer2 + case _timer2: return _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); + #endif + #ifdef _useTimer3 + case _timer3: return _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); + #endif + #ifdef _useTimer4 + case _timer4: return _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); + #endif + #ifdef _useTimer5 + case _timer5: return _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); + #endif + } +} + +void finISR(const timer16_Sequence_t timer_index) { + // Timer is disabled from the ISR, to ensure proper final pulse length. + DisablePending.set(timer_index); +} + +#endif // HAS_SERVOS + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/ServoTimers.h b/src/HAL/DUE/ServoTimers.h new file mode 100644 index 0000000..95bd404 --- /dev/null +++ b/src/HAL/DUE/ServoTimers.h @@ -0,0 +1,107 @@ +/** + * Copyright (c) 2013 Arduino LLC. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * Defines for 16 bit timers used with Servo library + * + * If _useTimerX is defined then TimerX is a 32 bit timer on the current board + * timer16_Sequence_t enumerates the sequence that the timers should be allocated + * _Nbr_16timers indicates how many timers are available. + */ + +/** + * SAM Only definitions + * -------------------- + */ + +// For SAM3X: +//!#define _useTimer1 +//!#define _useTimer2 +#define _useTimer3 +//!#define _useTimer4 +#define _useTimer5 + +#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays +#define SERVO_TIMER_PRESCALER 2 // timer prescaler + +/* + TC0, chan 0 => TC0_Handler + TC0, chan 1 => TC1_Handler + TC0, chan 2 => TC2_Handler + TC1, chan 0 => TC3_Handler + TC1, chan 1 => TC4_Handler + TC1, chan 2 => TC5_Handler + TC2, chan 0 => TC6_Handler + TC2, chan 1 => TC7_Handler + TC2, chan 2 => TC8_Handler + */ + +#ifdef _useTimer1 + #define TC_FOR_TIMER1 TC1 + #define CHANNEL_FOR_TIMER1 0 + #define ID_TC_FOR_TIMER1 ID_TC3 + #define IRQn_FOR_TIMER1 TC3_IRQn + #define HANDLER_FOR_TIMER1 TC3_Handler +#endif +#ifdef _useTimer2 + #define TC_FOR_TIMER2 TC1 + #define CHANNEL_FOR_TIMER2 1 + #define ID_TC_FOR_TIMER2 ID_TC4 + #define IRQn_FOR_TIMER2 TC4_IRQn + #define HANDLER_FOR_TIMER2 TC4_Handler +#endif +#ifdef _useTimer3 + #define TC_FOR_TIMER3 TC1 + #define CHANNEL_FOR_TIMER3 2 + #define ID_TC_FOR_TIMER3 ID_TC5 + #define IRQn_FOR_TIMER3 TC5_IRQn + #define HANDLER_FOR_TIMER3 TC5_Handler +#endif +#ifdef _useTimer4 + #define TC_FOR_TIMER4 TC0 + #define CHANNEL_FOR_TIMER4 2 + #define ID_TC_FOR_TIMER4 ID_TC2 + #define IRQn_FOR_TIMER4 TC2_IRQn + #define HANDLER_FOR_TIMER4 TC2_Handler +#endif +#ifdef _useTimer5 + #define TC_FOR_TIMER5 TC0 + #define CHANNEL_FOR_TIMER5 0 + #define ID_TC_FOR_TIMER5 ID_TC0 + #define IRQn_FOR_TIMER5 TC0_IRQn + #define HANDLER_FOR_TIMER5 TC0_Handler +#endif + +typedef enum : unsigned char { + #ifdef _useTimer1 + _timer1, + #endif + #ifdef _useTimer2 + _timer2, + #endif + #ifdef _useTimer3 + _timer3, + #endif + #ifdef _useTimer4 + _timer4, + #endif + #ifdef _useTimer5 + _timer5, + #endif + _Nbr_16timers +} timer16_Sequence_t; diff --git a/src/HAL/DUE/Tone.cpp b/src/HAL/DUE/Tone.cpp new file mode 100644 index 0000000..4bc8142 --- /dev/null +++ b/src/HAL/DUE/Tone.cpp @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Description: Tone function for Arduino Due and compatible (SAM3X8E) + * Derived from https://forum.arduino.cc/index.php?topic=136500.msg2903012#msg2903012 + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +static pin_t tone_pin; +volatile static int32_t toggles; + +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration/*=0*/) { + tone_pin = _pin; + toggles = 2 * frequency * duration / 1000; + HAL_timer_start(MF_TIMER_TONE, 2 * frequency); +} + +void noTone(const pin_t _pin) { + HAL_timer_disable_interrupt(MF_TIMER_TONE); + extDigitalWrite(_pin, LOW); +} + +HAL_TONE_TIMER_ISR() { + static uint8_t pin_state = 0; + HAL_timer_isr_prologue(MF_TIMER_TONE); + + if (toggles) { + toggles--; + extDigitalWrite(tone_pin, (pin_state ^= 1)); + } + else noTone(tone_pin); // turn off interrupt +} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp new file mode 100644 index 0000000..68f6a5c --- /dev/null +++ b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp @@ -0,0 +1,143 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_msp430_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2012, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __SAM3X8E__ + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include + +#include "../../../MarlinCore.h" + +#ifndef LCD_SPI_SPEED + #define LCD_SPI_SPEED SPI_QUARTER_SPEED +#endif + +#include "../../shared/HAL_SPI.h" +#include "../fastio.h" + +void u8g_SetPIOutput_DUE_hw_spi(u8g_t *u8g, uint8_t pin_index) { + PIO_Configure(g_APinDescription[u8g->pin_list[pin_index]].pPort, PIO_OUTPUT_1, + g_APinDescription[u8g->pin_list[pin_index]].ulPin, g_APinDescription[u8g->pin_list[pin_index]].ulPinConfiguration); // OUTPUT +} + +void u8g_SetPILevel_DUE_hw_spi(u8g_t *u8g, uint8_t pin_index, uint8_t level) { + volatile Pio* port = g_APinDescription[u8g->pin_list[pin_index]].pPort; + uint32_t mask = g_APinDescription[u8g->pin_list[pin_index]].ulPin; + if (level) port->PIO_SODR = mask; + else port->PIO_CODR = mask; +} + +uint8_t u8g_com_HAL_DUE_shared_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_INIT: + u8g_SetPILevel_DUE_hw_spi(u8g, U8G_PI_CS, 1); + u8g_SetPILevel_DUE_hw_spi(u8g, U8G_PI_A0, 1); + + u8g_SetPIOutput_DUE_hw_spi(u8g, U8G_PI_CS); + u8g_SetPIOutput_DUE_hw_spi(u8g, U8G_PI_A0); + + u8g_Delay(5); + + spiBegin(); + + spiInit(LCD_SPI_SPEED); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel_DUE_hw_spi(u8g, U8G_PI_A0, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + u8g_SetPILevel_DUE_hw_spi(u8g, U8G_PI_CS, (arg_val ? 0 : 1)); + break; + + case U8G_COM_MSG_RESET: + break; + + case U8G_COM_MSG_WRITE_BYTE: + + spiSend((uint8_t)arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + } + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB + +#endif // __SAM3X8E__ diff --git a/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp new file mode 100644 index 0000000..8268cf3 --- /dev/null +++ b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp @@ -0,0 +1,186 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../../inc/MarlinConfigPre.h" + +#if IS_U8GLIB_ST7920 + +#include "../../../inc/MarlinConfig.h" +#include "../../shared/Delay.h" + +#include + +#include "u8g_com_HAL_DUE_sw_spi_shared.h" + +#define SPISEND_SW_DUE u8g_spiSend_sw_DUE_mode_0 + +static uint8_t rs_last_state = 255; + +static void u8g_com_DUE_st7920_write_byte_sw_spi(uint8_t rs, uint8_t val) { + if (rs != rs_last_state) { // time to send a command/data byte + rs_last_state = rs; + SPISEND_SW_DUE(rs ? 0x0FA : 0x0F8); // Command or Data + DELAY_US(40); // give the controller some time to process the data: 20 is bad, 30 is OK, 40 is safe + } + SPISEND_SW_DUE(val & 0xF0); + SPISEND_SW_DUE(val << 4); +} + +uint8_t u8g_com_HAL_DUE_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SCK_pPio = g_APinDescription[u8g->pin_list[U8G_PI_SCK]].pPort; + SCK_dwMask = g_APinDescription[u8g->pin_list[U8G_PI_SCK]].ulPin; + MOSI_pPio = g_APinDescription[u8g->pin_list[U8G_PI_MOSI]].pPort; + MOSI_dwMask = g_APinDescription[u8g->pin_list[U8G_PI_MOSI]].ulPin; + + u8g_SetPILevel_DUE(u8g, U8G_PI_CS, 0); + u8g_SetPIOutput_DUE(u8g, U8G_PI_CS); + u8g_SetPILevel_DUE(u8g, U8G_PI_SCK, 0); + u8g_SetPIOutput_DUE(u8g, U8G_PI_SCK); + u8g_SetPILevel_DUE(u8g, U8G_PI_MOSI, 0); + u8g_SetPIOutput_DUE(u8g, U8G_PI_MOSI); + + SCK_pPio->PIO_CODR = SCK_dwMask; //SCK low - needed at power up but not after reset + MOSI_pPio->PIO_CODR = MOSI_dwMask; //MOSI low - needed at power up but not after reset + + u8g_Delay(5); + + u8g->pin_list[U8G_PI_A0_STATE] = 0; /* initial RS state: command mode */ + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel_DUE(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + break; + + case U8G_COM_MSG_CHIP_SELECT: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_CS]) + u8g_SetPILevel_DUE(u8g, U8G_PI_CS, arg_val); //note: the st7920 has an active high chip select + break; + + case U8G_COM_MSG_WRITE_BYTE: + + u8g_com_DUE_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_DUE_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_DUE_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + } + return 1; +} + +#if ENABLED(LIGHTWEIGHT_UI) + #include "../../../lcd/marlinui.h" + #include "../../shared/HAL_ST7920.h" + + #define ST7920_CS_PIN LCD_PINS_RS + + #if DOGM_SPI_DELAY_US > 0 + #define U8G_DELAY() DELAY_US(DOGM_SPI_DELAY_US) + #else + #define U8G_DELAY() DELAY_US(10) + #endif + + void ST7920_cs() { + WRITE(ST7920_CS_PIN, HIGH); + U8G_DELAY(); + } + + void ST7920_ncs() { + WRITE(ST7920_CS_PIN, LOW); + } + + void ST7920_set_cmd() { + SPISEND_SW_DUE(0xF8); + DELAY_US(40); + } + + void ST7920_set_dat() { + SPISEND_SW_DUE(0xFA); + DELAY_US(40); + } + + void ST7920_write_byte(const uint8_t val) { + SPISEND_SW_DUE(val & 0xF0); + SPISEND_SW_DUE(val << 4); + } +#endif // LIGHTWEIGHT_UI + +#endif // IS_U8GLIB_ST7920 +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp new file mode 100644 index 0000000..68e3e74 --- /dev/null +++ b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp @@ -0,0 +1,145 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_std_sw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2015, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#include "u8g_com_HAL_DUE_sw_spi_shared.h" + +#include "../../shared/Marduino.h" +#include "../../shared/Delay.h" + +#include + +#if ENABLED(FYSETC_MINI_12864) + #define SPISEND_SW_DUE u8g_spiSend_sw_DUE_mode_3 +#else + #define SPISEND_SW_DUE u8g_spiSend_sw_DUE_mode_0 +#endif + +uint8_t u8g_com_HAL_DUE_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SCK_pPio = g_APinDescription[u8g->pin_list[U8G_PI_SCK]].pPort; + SCK_dwMask = g_APinDescription[u8g->pin_list[U8G_PI_SCK]].ulPin; + MOSI_pPio = g_APinDescription[u8g->pin_list[U8G_PI_MOSI]].pPort; + MOSI_dwMask = g_APinDescription[u8g->pin_list[U8G_PI_MOSI]].ulPin; + u8g_SetPIOutput_DUE(u8g, U8G_PI_SCK); + u8g_SetPIOutput_DUE(u8g, U8G_PI_MOSI); + u8g_SetPIOutput_DUE(u8g, U8G_PI_CS); + u8g_SetPIOutput_DUE(u8g, U8G_PI_A0); + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPIOutput_DUE(u8g, U8G_PI_RESET); + u8g_SetPILevel_DUE(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel_DUE(u8g, U8G_PI_MOSI, 0); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel_DUE(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if ENABLED(FYSETC_MINI_12864) // LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + u8g_SetPILevel_DUE(u8g, U8G_PI_SCK, 1); //set SCK to mode 3 idle state before CS goes active + u8g_SetPILevel_DUE(u8g, U8G_PI_CS, LOW); + } + else { + u8g_SetPILevel_DUE(u8g, U8G_PI_CS, HIGH); + u8g_SetPILevel_DUE(u8g, U8G_PI_SCK, 0); //set SCK to mode 0 idle state after CS goes inactive + } + #else + u8g_SetPILevel_DUE(u8g, U8G_PI_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + SPISEND_SW_DUE(arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + SPISEND_SW_DUE(*ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + SPISEND_SW_DUE(u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel_DUE(u8g, U8G_PI_A0, arg_val); + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp new file mode 100644 index 0000000..9049247 --- /dev/null +++ b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include "../../../inc/MarlinConfig.h" +#include "../../shared/Delay.h" + +#include + +#include "u8g_com_HAL_DUE_sw_spi_shared.h" + +void u8g_SetPIOutput_DUE(u8g_t *u8g, uint8_t pin_index) { + PIO_Configure(g_APinDescription[u8g->pin_list[pin_index]].pPort, PIO_OUTPUT_1, + g_APinDescription[u8g->pin_list[pin_index]].ulPin, g_APinDescription[u8g->pin_list[pin_index]].ulPinConfiguration); // OUTPUT +} + +void u8g_SetPILevel_DUE(u8g_t *u8g, uint8_t pin_index, uint8_t level) { + volatile Pio* port = g_APinDescription[u8g->pin_list[pin_index]].pPort; + uint32_t mask = g_APinDescription[u8g->pin_list[pin_index]].ulPin; + if (level) port->PIO_SODR = mask; else port->PIO_CODR = mask; +} + +Pio *SCK_pPio, *MOSI_pPio; +uint32_t SCK_dwMask, MOSI_dwMask; + +void u8g_spiSend_sw_DUE_mode_0(uint8_t val) { // 3MHz + LOOP_L_N(i, 8) { + if (val & 0x80) + MOSI_pPio->PIO_SODR = MOSI_dwMask; + else + MOSI_pPio->PIO_CODR = MOSI_dwMask; + DELAY_NS(48); + SCK_pPio->PIO_SODR = SCK_dwMask; + DELAY_NS(905); + val <<= 1; + SCK_pPio->PIO_CODR = SCK_dwMask; + } +} + +void u8g_spiSend_sw_DUE_mode_3(uint8_t val) { // 3.5MHz + LOOP_L_N(i, 8) { + SCK_pPio->PIO_CODR = SCK_dwMask; + DELAY_NS(50); + if (val & 0x80) + MOSI_pPio->PIO_SODR = MOSI_dwMask; + else + MOSI_pPio->PIO_CODR = MOSI_dwMask; + val <<= 1; + DELAY_NS(10); + SCK_pPio->PIO_SODR = SCK_dwMask; + DELAY_NS(70); + } +} + +#endif // HAS_MARLINUI_U8GLIB +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h new file mode 100644 index 0000000..45231fd --- /dev/null +++ b/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h @@ -0,0 +1,35 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfigPre.h" +#include "../../shared/Marduino.h" +#include + +void u8g_SetPIOutput_DUE(u8g_t *u8g, uint8_t pin_index); +void u8g_SetPILevel_DUE(u8g_t *u8g, uint8_t pin_index, uint8_t level); + +void u8g_spiSend_sw_DUE_mode_0(uint8_t val); +void u8g_spiSend_sw_DUE_mode_3(uint8_t val); + +extern Pio *SCK_pPio, *MOSI_pPio; +extern uint32_t SCK_dwMask, MOSI_dwMask; diff --git a/src/HAL/DUE/eeprom_flash.cpp b/src/HAL/DUE/eeprom_flash.cpp new file mode 100644 index 0000000..6077641 --- /dev/null +++ b/src/HAL/DUE/eeprom_flash.cpp @@ -0,0 +1,998 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FLASH_EEPROM_EMULATION) + +/* EEPROM emulation over flash with reduced wear + * + * We will use 2 contiguous groups of pages as main and alternate. + * We want an structure that allows to read as fast as possible, + * without the need of scanning the whole FLASH memory. + * + * FLASH bits default erased state is 1, and can be set to 0 + * on a per bit basis. To reset them to 1, a full page erase + * is needed. + * + * Values are stored as differences that should be applied to a + * completely erased EEPROM (filled with 0xFFs). We just encode + * the starting address of the values to change, the length of + * the block of new values, and the values themselves. All diffs + * are accumulated into a RAM buffer, compacted into the least + * amount of non overlapping diffs possible and sorted by starting + * address before being saved into the next available page of FLASH + * of the current group. + * Once the current group is completely full, we compact it and save + * it into the other group, then erase the current group and switch + * to that new group and set it as current. + * + * The FLASH endurance is about 1/10 ... 1/100 of an EEPROM + * endurance, but EEPROM endurance is specified per byte, not + * per page. We can't emulate EE endurance with FLASH for all + * bytes, but we can emulate endurance for a given percent of + * bytes. + */ + +//#define EE_EMU_DEBUG + +#define EEPROMSize 4096 +#define PagesPerGroup 128 +#define GroupCount 2 +#define PageSize 256U + + /* Flash storage */ +typedef struct FLASH_SECTOR { + uint8_t page[PageSize]; +} FLASH_SECTOR_T; + +#define PAGE_FILL \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, \ + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF + +#define FLASH_INIT_FILL \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL, \ + PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL,PAGE_FILL + +/* This is the FLASH area used to emulate a 2Kbyte EEPROM -- We need this buffer aligned + to a 256 byte boundary. */ +static const uint8_t flashStorage[PagesPerGroup * GroupCount * PageSize] __attribute__ ((aligned (PageSize))) = { FLASH_INIT_FILL }; + +/* Get the address of an specific page */ +static const FLASH_SECTOR_T* getFlashStorage(int page) { + return (const FLASH_SECTOR_T*)&flashStorage[page*PageSize]; +} + +static uint8_t buffer[256] = {0}, // The RAM buffer to accumulate writes + curPage = 0, // Current FLASH page inside the group + curGroup = 0xFF; // Current FLASH group + +#define DEBUG_OUT ENABLED(EE_EMU_DEBUG) +#include "../../core/debug_out.h" + +static void ee_Dump(const int page, const void *data) { + + #ifdef EE_EMU_DEBUG + + const uint8_t *c = (const uint8_t*) data; + char buffer[80]; + + sprintf_P(buffer, PSTR("Page: %d (0x%04x)\n"), page, page); + DEBUG_ECHO(buffer); + + char* p = &buffer[0]; + for (int i = 0; i< PageSize; ++i) { + if ((i & 0xF) == 0) p += sprintf_P(p, PSTR("%04x] "), i); + + p += sprintf_P(p, PSTR(" %02x"), c[i]); + if ((i & 0xF) == 0xF) { + *p++ = '\n'; + *p = 0; + DEBUG_ECHO(buffer); + p = &buffer[0]; + } + } + + #else + UNUSED(page); + UNUSED(data); + #endif +} + +/* Flash Writing Protection Key */ +#define FWP_KEY 0x5Au + +#if SAM4S_SERIES + #define EEFC_FCR_FCMD(value) \ + ((EEFC_FCR_FCMD_Msk & ((value) << EEFC_FCR_FCMD_Pos))) + #define EEFC_ERROR_FLAGS (EEFC_FSR_FLOCKE | EEFC_FSR_FCMDE | EEFC_FSR_FLERR) +#else + #define EEFC_ERROR_FLAGS (EEFC_FSR_FLOCKE | EEFC_FSR_FCMDE) +#endif + +/** + * Writes the contents of the specified page (no previous erase) + * @param page (page #) + * @param data (pointer to the data buffer) + */ +__attribute__ ((long_call, section (".ramfunc"))) +static bool ee_PageWrite(uint16_t page, const void *data) { + + uint16_t i; + uint32_t addrflash = uint32_t(getFlashStorage(page)); + + // Read the flash contents + uint32_t pageContents[PageSize>>2]; + memcpy(pageContents, (void*)addrflash, PageSize); + + // We ONLY want to toggle bits that have changed, and that have changed to 0. + // SAM3X8E tends to destroy contiguous bits if reprogrammed without erasing, so + // we try by all means to avoid this. That is why it says: "The Partial + // Programming mode works only with 128-bit (or higher) boundaries. It cannot + // be used with boundaries lower than 128 bits (8, 16 or 32-bit for example)." + // All bits that did not change, set them to 1. + for (i = 0; i > 2; i++) + pageContents[i] = (((uint32_t*)data)[i]) | (~(pageContents[i] ^ ((uint32_t*)data)[i])); + + DEBUG_ECHO_MSG("EEPROM PageWrite ", page); + DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash); + DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0)); + DEBUG_FLUSH(); + + // Get the page relative to the start of the EFC controller, and the EFC controller to use + Efc *efc; + uint16_t fpage; + if (addrflash >= IFLASH1_ADDR) { + efc = EFC1; + fpage = (addrflash - IFLASH1_ADDR) / IFLASH1_PAGE_SIZE; + } + else { + efc = EFC0; + fpage = (addrflash - IFLASH0_ADDR) / IFLASH0_PAGE_SIZE; + } + + // Get the page that must be unlocked, then locked + uint16_t lpage = fpage & (~((IFLASH0_LOCK_REGION_SIZE / IFLASH0_PAGE_SIZE) - 1)); + + // Disable all interrupts + __disable_irq(); + + // Get the FLASH wait states + uint32_t orgWS = (efc->EEFC_FMR & EEFC_FMR_FWS_Msk) >> EEFC_FMR_FWS_Pos; + + // Set wait states to 6 (SAM errata) + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(6); + + // Unlock the flash page + uint32_t status; + efc->EEFC_FCR = EEFC_FCR_FKEY(FWP_KEY) | EEFC_FCR_FARG(lpage) | EEFC_FCR_FCMD(EFC_FCMD_CLB); + while (((status = efc->EEFC_FSR) & EEFC_FSR_FRDY) != EEFC_FSR_FRDY) { + // force compiler to not optimize this -- NOPs don't work! + __asm__ __volatile__(""); + }; + + if ((status & EEFC_ERROR_FLAGS) != 0) { + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + DEBUG_ECHO_MSG("EEPROM Unlock failure for page ", page); + return false; + } + + // Write page and lock: Writing 8-bit and 16-bit data is not allowed and may lead to unpredictable data corruption. + const uint32_t * aligned_src = (const uint32_t *) &pageContents[0]; /*data;*/ + uint32_t * p_aligned_dest = (uint32_t *) addrflash; + for (i = 0; i < (IFLASH0_PAGE_SIZE / sizeof(uint32_t)); ++i) { + *p_aligned_dest++ = *aligned_src++; + } + efc->EEFC_FCR = EEFC_FCR_FKEY(FWP_KEY) | EEFC_FCR_FARG(fpage) | EEFC_FCR_FCMD(EFC_FCMD_WPL); + while (((status = efc->EEFC_FSR) & EEFC_FSR_FRDY) != EEFC_FSR_FRDY) { + // force compiler to not optimize this -- NOPs don't work! + __asm__ __volatile__(""); + }; + + if ((status & EEFC_ERROR_FLAGS) != 0) { + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + DEBUG_ECHO_MSG("EEPROM Write failure for page ", page); + + return false; + } + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + // Compare contents + if (memcmp(getFlashStorage(page),data,PageSize)) { + + #ifdef EE_EMU_DEBUG + DEBUG_ECHO_MSG("EEPROM Verify Write failure for page ", page); + + ee_Dump( page, (uint32_t *)addrflash); + ee_Dump(-page, data); + + // Calculate count of changed bits + uint32_t *p1 = (uint32_t*)addrflash; + uint32_t *p2 = (uint32_t*)data; + int count = 0; + for (i =0; i> 2; i++) { + if (p1[i] != p2[i]) { + uint32_t delta = p1[i] ^ p2[i]; + while (delta) { + if ((delta&1) != 0) + count++; + delta >>= 1; + } + } + } + DEBUG_ECHOLNPGM("--> Differing bits: ", count); + #endif + + return false; + } + + return true; +} + +/** + * Erases the contents of the specified page + * @param page (page #) + */ +__attribute__ ((long_call, section (".ramfunc"))) +static bool ee_PageErase(uint16_t page) { + + uint16_t i; + uint32_t addrflash = uint32_t(getFlashStorage(page)); + + DEBUG_ECHO_MSG("EEPROM PageErase ", page); + DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash); + DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0)); + DEBUG_FLUSH(); + + // Get the page relative to the start of the EFC controller, and the EFC controller to use + Efc *efc; + uint16_t fpage; + if (addrflash >= IFLASH1_ADDR) { + efc = EFC1; + fpage = (addrflash - IFLASH1_ADDR) / IFLASH1_PAGE_SIZE; + } + else { + efc = EFC0; + fpage = (addrflash - IFLASH0_ADDR) / IFLASH0_PAGE_SIZE; + } + + // Get the page that must be unlocked, then locked + uint16_t lpage = fpage & (~((IFLASH0_LOCK_REGION_SIZE / IFLASH0_PAGE_SIZE) - 1)); + + // Disable all interrupts + __disable_irq(); + + // Get the FLASH wait states + uint32_t orgWS = (efc->EEFC_FMR & EEFC_FMR_FWS_Msk) >> EEFC_FMR_FWS_Pos; + + // Set wait states to 6 (SAM errata) + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(6); + + // Unlock the flash page + uint32_t status; + efc->EEFC_FCR = EEFC_FCR_FKEY(FWP_KEY) | EEFC_FCR_FARG(lpage) | EEFC_FCR_FCMD(EFC_FCMD_CLB); + while (((status = efc->EEFC_FSR) & EEFC_FSR_FRDY) != EEFC_FSR_FRDY) { + // force compiler to not optimize this -- NOPs don't work! + __asm__ __volatile__(""); + }; + if ((status & EEFC_ERROR_FLAGS) != 0) { + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + DEBUG_ECHO_MSG("EEPROM Unlock failure for page ",page); + + return false; + } + + // Erase Write page and lock: Writing 8-bit and 16-bit data is not allowed and may lead to unpredictable data corruption. + uint32_t * p_aligned_dest = (uint32_t *) addrflash; + for (i = 0; i < (IFLASH0_PAGE_SIZE / sizeof(uint32_t)); ++i) { + *p_aligned_dest++ = 0xFFFFFFFF; + } + efc->EEFC_FCR = EEFC_FCR_FKEY(FWP_KEY) | EEFC_FCR_FARG(fpage) | EEFC_FCR_FCMD(EFC_FCMD_EWPL); + while (((status = efc->EEFC_FSR) & EEFC_FSR_FRDY) != EEFC_FSR_FRDY) { + // force compiler to not optimize this -- NOPs don't work! + __asm__ __volatile__(""); + }; + if ((status & EEFC_ERROR_FLAGS) != 0) { + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + DEBUG_ECHO_MSG("EEPROM Erase failure for page ",page); + + return false; + } + + // Restore original wait states + efc->EEFC_FMR = (efc->EEFC_FMR & (~EEFC_FMR_FWS_Msk)) | EEFC_FMR_FWS(orgWS); + + // Reenable interrupts + __enable_irq(); + + // Check erase + uint32_t * aligned_src = (uint32_t *) addrflash; + for (i = 0; i < PageSize >> 2; i++) { + if (*aligned_src++ != 0xFFFFFFFF) { + DEBUG_ECHO_MSG("EEPROM Verify Erase failure for page ",page); + ee_Dump(page, (uint32_t *)addrflash); + return false; + } + } + + return true; +} + +static uint8_t ee_Read(uint32_t address, bool excludeRAMBuffer=false) { + + uint32_t baddr; + uint32_t blen; + + // If we were requested an address outside of the emulated range, fail now + if (address >= EEPROMSize) + return false; + + // Check that the value is not contained in the RAM buffer + if (!excludeRAMBuffer) { + uint16_t i = 0; + while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Get the address of the block + baddr = buffer[i] | (buffer[i + 1] << 8); + + // Get the length of the block + blen = buffer[i + 2]; + + // If we reach the end of the list, break loop + if (blen == 0xFF) + break; + + // Check if data is contained in this block + if (address >= baddr && + address < (baddr + blen)) { + + // Yes, it is contained. Return it! + return buffer[i + 3 + address - baddr]; + } + + // As blocks are always sorted, if the starting address of this block is higher + // than the address we are looking for, break loop now - We wont find the value + // associated to the address + if (baddr > address) + break; + + // Jump to the next block + i += 3 + blen; + } + } + + // It is NOT on the RAM buffer. It could be stored in FLASH. We are + // ensured on a given FLASH page, address contents are never repeated + // but on different pages, there is no such warranty, so we must go + // backwards from the last written FLASH page to the first one. + for (int page = curPage - 1; page >= 0; --page) { + + // Get a pointer to the flash page + uint8_t *pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); + + uint16_t i = 0; + while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Get the address of the block + baddr = pflash[i] | (pflash[i + 1] << 8); + + // Get the length of the block + blen = pflash[i + 2]; + + // If we reach the end of the list, break loop + if (blen == 0xFF) + break; + + // Check if data is contained in this block + if (address >= baddr && address < (baddr + blen)) + return pflash[i + 3 + address - baddr]; // Yes, it is contained. Return it! + + // As blocks are always sorted, if the starting address of this block is higher + // than the address we are looking for, break loop now - We wont find the value + // associated to the address + if (baddr > address) break; + + // Jump to the next block + i += 3 + blen; + } + } + + // If reached here, value is not stored, so return its default value + return 0xFF; +} + +static uint32_t ee_GetAddrRange(uint32_t address, bool excludeRAMBuffer=false) { + uint32_t baddr, + blen, + nextAddr = 0xFFFF, + nextRange = 0; + + // Check that the value is not contained in the RAM buffer + if (!excludeRAMBuffer) { + uint16_t i = 0; + while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Get the address of the block + baddr = buffer[i] | (buffer[i + 1] << 8); + + // Get the length of the block + blen = buffer[i + 2]; + + // If we reach the end of the list, break loop + if (blen == 0xFF) break; + + // Check if address and address + 1 is contained in this block + if (address >= baddr && address < (baddr + blen)) + return address | ((blen - address + baddr) << 16); // Yes, it is contained. Return it! + + // Otherwise, check if we can use it as a limit + if (baddr > address && baddr < nextAddr) { + nextAddr = baddr; + nextRange = blen; + } + + // As blocks are always sorted, if the starting address of this block is higher + // than the address we are looking for, break loop now - We wont find the value + // associated to the address + if (baddr > address) break; + + // Jump to the next block + i += 3 + blen; + } + } + + // It is NOT on the RAM buffer. It could be stored in FLASH. We are + // ensured on a given FLASH page, address contents are never repeated + // but on different pages, there is no such warranty, so we must go + // backwards from the last written FLASH page to the first one. + for (int page = curPage - 1; page >= 0; --page) { + + // Get a pointer to the flash page + uint8_t *pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); + + uint16_t i = 0; + while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Get the address of the block + baddr = pflash[i] | (pflash[i + 1] << 8); + + // Get the length of the block + blen = pflash[i + 2]; + + // If we reach the end of the list, break loop + if (blen == 0xFF) break; + + // Check if data is contained in this block + if (address >= baddr && address < (baddr + blen)) + return address | ((blen - address + baddr) << 16); // Yes, it is contained. Return it! + + // Otherwise, check if we can use it as a limit + if (baddr > address && baddr < nextAddr) { + nextAddr = baddr; + nextRange = blen; + } + + // As blocks are always sorted, if the starting address of this block is higher + // than the address we are looking for, break loop now - We wont find the value + // associated to the address + if (baddr > address) break; + + // Jump to the next block + i += 3 + blen; + } + } + + // If reached here, we will return the next valid address + return nextAddr | (nextRange << 16); +} + +static bool ee_IsPageClean(int page) { + uint32_t *pflash = (uint32_t*) getFlashStorage(page); + for (uint16_t i = 0; i < (PageSize >> 2); ++i) + if (*pflash++ != 0xFFFFFFFF) return false; + return true; +} + +static bool ee_Flush(uint32_t overrideAddress = 0xFFFFFFFF, uint8_t overrideData=0xFF) { + + // Check if RAM buffer has something to be written + bool isEmpty = true; + uint32_t *p = (uint32_t*) &buffer[0]; + for (uint16_t j = 0; j < (PageSize >> 2); j++) { + if (*p++ != 0xFFFFFFFF) { + isEmpty = false; + break; + } + } + + // If something has to be written, do so! + if (!isEmpty) { + + // Write the current ram buffer into FLASH + ee_PageWrite(curPage + curGroup * PagesPerGroup, buffer); + + // Clear the RAM buffer + memset(buffer, 0xFF, sizeof(buffer)); + + // Increment the page to use the next time + ++curPage; + } + + // Did we reach the maximum count of available pages per group for storage ? + if (curPage < PagesPerGroup) { + + // Do we have an override address ? + if (overrideAddress < EEPROMSize) { + + // Yes, just store the value into the RAM buffer + buffer[0] = overrideAddress & 0xFF; + buffer[0 + 1] = (overrideAddress >> 8) & 0xFF; + buffer[0 + 2] = 1; + buffer[0 + 3] = overrideData; + } + + // Done! + return true; + } + + // We have no space left on the current group - We must compact the values + uint16_t i = 0; + + // Compute the next group to use + int curwPage = 0, curwGroup = curGroup + 1; + if (curwGroup >= GroupCount) curwGroup = 0; + + uint32_t rdAddr = 0; + do { + + // Get the next valid range + uint32_t addrRange = ee_GetAddrRange(rdAddr, true); + + // Make sure not to skip the override address, if specified + int rdRange; + if (overrideAddress < EEPROMSize && + rdAddr <= overrideAddress && + (addrRange & 0xFFFF) > overrideAddress) { + + rdAddr = overrideAddress; + rdRange = 1; + } + else { + rdAddr = addrRange & 0xFFFF; + rdRange = addrRange >> 16; + } + + // If no range, break loop + if (rdRange == 0) + break; + + do { + + // Get the value + uint8_t rdValue = overrideAddress == rdAddr ? overrideData : ee_Read(rdAddr, true); + + // Do not bother storing default values + if (rdValue != 0xFF) { + + // If we have room, add it to the buffer + if (buffer[i + 2] == 0xFF) { + + // Uninitialized buffer, just add it! + buffer[i] = rdAddr & 0xFF; + buffer[i + 1] = (rdAddr >> 8) & 0xFF; + buffer[i + 2] = 1; + buffer[i + 3] = rdValue; + + } + else { + // Buffer already has contents. Check if we can extend it + + // Get the address of the block + uint32_t baddr = buffer[i] | (buffer[i + 1] << 8); + + // Get the length of the block + uint32_t blen = buffer[i + 2]; + + // Can we expand it ? + if (rdAddr == (baddr + blen) && + i < (PageSize - 4) && /* This block has a chance to contain data AND */ + buffer[i + 2] < (PageSize - i - 3)) {/* There is room for this block to be expanded */ + + // Yes, do it + ++buffer[i + 2]; + + // And store the value + buffer[i + 3 + rdAddr - baddr] = rdValue; + + } + else { + + // No, we can't expand it - Skip the existing block + i += 3 + blen; + + // Can we create a new slot ? + if (i > (PageSize - 4)) { + + // Not enough space - Write the current buffer to FLASH + ee_PageWrite(curwPage + curwGroup * PagesPerGroup, buffer); + + // Advance write page (as we are compacting, should never overflow!) + ++curwPage; + + // Clear RAM buffer + memset(buffer, 0xFF, sizeof(buffer)); + + // Start fresh */ + i = 0; + } + + // Enough space, add the new block + buffer[i] = rdAddr & 0xFF; + buffer[i + 1] = (rdAddr >> 8) & 0xFF; + buffer[i + 2] = 1; + buffer[i + 3] = rdValue; + } + } + } + + // Go to the next address + ++rdAddr; + + // Repeat for bytes of this range + } while (--rdRange); + + // Repeat until we run out of ranges + } while (rdAddr < EEPROMSize); + + // We must erase the previous group, in preparation for the next swap + for (int page = 0; page < curPage; page++) { + ee_PageErase(page + curGroup * PagesPerGroup); + } + + // Finally, Now the active group is the created new group + curGroup = curwGroup; + curPage = curwPage; + + // Done! + return true; +} + +static bool ee_Write(uint32_t address, uint8_t data) { + + // If we were requested an address outside of the emulated range, fail now + if (address >= EEPROMSize) return false; + + // Lets check if we have a block with that data previously defined. Block + // start addresses are always sorted in ascending order + uint16_t i = 0; + while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Get the address of the block + uint32_t baddr = buffer[i] | (buffer[i + 1] << 8); + + // Get the length of the block + uint32_t blen = buffer[i + 2]; + + // If we reach the end of the list, break loop + if (blen == 0xFF) + break; + + // Check if data is contained in this block + if (address >= baddr && + address < (baddr + blen)) { + + // Yes, it is contained. Just modify it + buffer[i + 3 + address - baddr] = data; + + // Done! + return true; + } + + // Maybe we could add it to the front or to the back + // of this block ? + if ((address + 1) == baddr || address == (baddr + blen)) { + + // Potentially, it could be done. But we must ensure there is room + // so we can expand the block. Lets find how much free space remains + uint32_t iend = i; + do { + uint32_t ln = buffer[iend + 2]; + if (ln == 0xFF) break; + iend += 3 + ln; + } while (iend <= (PageSize - 4)); /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + + // Here, inxt points to the first free address in the buffer. Do we have room ? + if (iend < PageSize) { + // Yes, at least a byte is free - We can expand the block + + // Do we have to insert at the beginning ? + if ((address + 1) == baddr) { + + // Insert at the beginning + + // Make room at the beginning for our byte + memmove(&buffer[i + 3 + 1], &buffer[i + 3], iend - i - 3); + + // Adjust the header and store the data + buffer[i] = address & 0xFF; + buffer[i + 1] = (address >> 8) & 0xFF; + buffer[i + 2]++; + buffer[i + 3] = data; + + } + else { + + // Insert at the end - There is a very interesting thing that could happen here: + // Maybe we could coalesce the next block with this block. Let's try to do it! + uint16_t inext = i + 3 + blen; + if (inext <= (PageSize - 4) && + (buffer[inext] | uint16_t(buffer[inext + 1] << 8)) == (baddr + blen + 1)) { + // YES! ... we can coalesce blocks! . Do it! + + // Adjust this block header to include the next one + buffer[i + 2] += buffer[inext + 2] + 1; + + // Store data at the right place + buffer[i + 3 + blen] = data; + + // Remove the next block header and append its data + memmove(&buffer[inext + 1], &buffer[inext + 3], iend - inext - 3); + + // Finally, as we have saved 2 bytes at the end, make sure to clean them + buffer[iend - 2] = 0xFF; + buffer[iend - 1] = 0xFF; + + } + else { + // NO ... No coalescing possible yet + + // Make room at the end for our byte + memmove(&buffer[i + 3 + blen + 1], &buffer[i + 3 + blen], iend - i - 3 - blen); + + // And add the data to the block + buffer[i + 2]++; + buffer[i + 3 + blen] = data; + } + } + + // Done! + return true; + } + } + + // As blocks are always sorted, if the starting address of this block is higher + // than the address we are looking for, break loop now - We wont find the value + // associated to the address + if (baddr > address) break; + + // Jump to the next block + i += 3 + blen; + } + + // Value is not stored AND we can't expand previous block to contain it. We must create a new block + + // First, lets find how much free space remains + uint32_t iend = i; + while (iend <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ + uint32_t ln = buffer[iend + 2]; + if (ln == 0xFF) break; + iend += 3 + ln; + } + + // If there is room for a new block, insert it at the proper place + if (iend <= (PageSize - 4)) { + + // We have room to create a new block. Do so --- But add + // the block at the proper position, sorted by starting + // address, so it will be possible to compact it with other blocks. + + // Make space + memmove(&buffer[i + 4], &buffer[i], iend - i); + + // And add the block + buffer[i] = address & 0xFF; + buffer[i + 1] = (address >> 8) & 0xFF; + buffer[i + 2] = 1; + buffer[i + 3] = data; + + // Done! + return true; + } + + // Not enough room to store this information on this FLASH page - Perform a + // flush and override the address with the specified contents + return ee_Flush(address, data); +} + +static void ee_Init() { + + // Just init once! + if (curGroup != 0xFF) return; + + // Clean up the SRAM buffer + memset(buffer, 0xFF, sizeof(buffer)); + + // Now, we must find out the group where settings are stored + for (curGroup = 0; curGroup < GroupCount; curGroup++) + if (!ee_IsPageClean(curGroup * PagesPerGroup)) break; + + // If all groups seem to be used, default to first group + if (curGroup >= GroupCount) curGroup = 0; + + DEBUG_ECHO_MSG("EEPROM Current Group: ",curGroup); + DEBUG_FLUSH(); + + // Now, validate that all the other group pages are empty + for (int grp = 0; grp < GroupCount; grp++) { + if (grp == curGroup) continue; + + for (int page = 0; page < PagesPerGroup; page++) { + if (!ee_IsPageClean(grp * PagesPerGroup + page)) { + DEBUG_ECHO_MSG("EEPROM Page ", page, " not clean on group ", grp); + DEBUG_FLUSH(); + ee_PageErase(grp * PagesPerGroup + page); + } + } + } + + // Finally, for the active group, determine the first unused page + // and also validate that all the other ones are clean + for (curPage = 0; curPage < PagesPerGroup; curPage++) { + if (ee_IsPageClean(curGroup * PagesPerGroup + curPage)) { + ee_Dump(curGroup * PagesPerGroup + curPage, getFlashStorage(curGroup * PagesPerGroup + curPage)); + break; + } + } + + DEBUG_ECHO_MSG("EEPROM Active page: ", curPage); + DEBUG_FLUSH(); + + // Make sure the pages following the first clean one are also clean + for (int page = curPage + 1; page < PagesPerGroup; page++) { + if (!ee_IsPageClean(curGroup * PagesPerGroup + page)) { + DEBUG_ECHO_MSG("EEPROM Page ", page, " not clean on active group ", curGroup); + DEBUG_FLUSH(); + ee_Dump(curGroup * PagesPerGroup + page, getFlashStorage(curGroup * PagesPerGroup + page)); + ee_PageErase(curGroup * PagesPerGroup + page); + } + } +} + +/* PersistentStore -----------------------------------------------------------*/ + +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } +bool PersistentStore::access_start() { ee_Init(); return true; } +bool PersistentStore::access_finish() { ee_Flush(); return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != ee_Read(uint32_t(p))) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + ee_Write(uint32_t(p), v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (ee_Read(uint32_t(p)) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = ee_Read(uint32_t(pos)); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // FLASH_EEPROM_EMULATION +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/eeprom_wired.cpp b/src/HAL/DUE/eeprom_wired.cpp new file mode 100644 index 0000000..557a2f2 --- /dev/null +++ b/src/HAL/DUE/eeprom_wired.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_SAM + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for I2C / SPI EEPROM." +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/endstop_interrupts.h b/src/HAL/DUE/endstop_interrupts.h new file mode 100644 index 0000000..9c7e210 --- /dev/null +++ b/src/HAL/DUE/endstop_interrupts.h @@ -0,0 +1,73 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +/** + * Endstop interrupts for Due based targets. + * On Due, all pins support external interrupt capability. + */ + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/DUE/fastio.h b/src/HAL/DUE/fastio.h new file mode 100644 index 0000000..a609210 --- /dev/null +++ b/src/HAL/DUE/fastio.h @@ -0,0 +1,565 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for SAM3X8E + * Use direct port manipulation to save scads of processor time. + * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al. + */ + +/** + * Description: Fast IO functions for Arduino Due and compatible (SAM3X8E) + * + * For ARDUINO_ARCH_SAM + * Note the code here was specifically crafted by disassembling what GCC produces + * out of it, so GCC is able to optimize it out as much as possible to the least + * amount of instructions. Be very careful if you modify them, as "clean code" + * leads to less efficient compiled code!! + */ + +#include + +#include "../../inc/MarlinConfigPre.h" + +/** + * Utility functions + */ + +// Due has 12 PWMs assigned to logical pins 2-13. +// 6, 7, 8 & 9 come from the PWM controller. The others come from the timers. +#define PWM_PIN(P) WITHIN(P, 2, 13) + +#ifndef MASK + #define MASK(PIN) _BV(PIN) +#endif + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(STEP); WRITE(STEP, HIGH); WRITE(STEP, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/cpp/Stringification.html + */ + +// Read a pin +#define _READ(IO) bool(DIO ## IO ## _WPORT -> PIO_PDSR & MASK(DIO ## IO ## _PIN)) + +// Write to a pin +#define _WRITE(IO,V) do { \ + volatile Pio* port = (DIO ## IO ## _WPORT); \ + const uint32_t mask = MASK(DIO ## IO ## _PIN); \ + if (V) port->PIO_SODR = mask; \ + else port->PIO_CODR = mask; \ +}while(0) + +// Toggle a pin +#define _TOGGLE(IO) _WRITE(IO, !READ(IO)) + +#if MB(PRINTRBOARD_G2) + + #include "fastio/G2_pins.h" + + // Set pin as input + #define _SET_INPUT(IO) do{ \ + pmc_enable_periph_clk(G2_g_APinDescription[IO].ulPeripheralId); \ + PIO_Configure((DIO ## IO ## _WPORT), PIO_INPUT, MASK(DIO ## IO ## _PIN), 0); \ + }while(0) + + // Set pin as output + #define _SET_OUTPUT(IO) do{ \ + uint32_t mask = MASK(G2_g_APinDescription[IO].ulPeripheralId); \ + if ((PMC->PMC_PCSR0 & mask) != (mask)) PMC->PMC_PCER0 = mask; \ + volatile Pio* port = (DIO ## IO ## _WPORT); \ + mask = MASK(DIO ## IO ## _PIN); \ + if (_READ(IO)) port->PIO_SODR = mask; \ + else port->PIO_CODR = mask; \ + port->PIO_IDR = mask; \ + const uint32_t pin_config = G2_g_APinDescription[IO].ulPinConfiguration; \ + if (pin_config & PIO_PULLUP) port->PIO_PUER = mask; \ + else port->PIO_PUDR = mask; \ + if (pin_config & PIO_OPENDRAIN) port->PIO_MDER = mask; \ + else port->PIO_MDDR = mask; \ + port->PIO_PER = mask; \ + port->PIO_OER = mask; \ + g_pinStatus[IO] = (g_pinStatus[IO] & 0xF0) | PIN_STATUS_DIGITAL_OUTPUT; \ + }while(0) + + /** + * Set pin as output with comments + * #define _SET_OUTPUT(IO) do{ \ + * uint32_t mask = MASK(G2_g_APinDescription[IO].ulPeripheralId); \ + * if ((PMC->PMC_PCSR0 & mask ) != (mask)) PMC->PMC_PCER0 = mask; \ // enable PIO clock if not already enabled + * + * volatile Pio* port = (DIO ## IO ## _WPORT); \ + * const uint32_t mask = MASK(DIO ## IO ## _PIN); \ + * if (_READ(IO)) port->PIO_SODR = mask; \ // set output to match input BEFORE setting direction or will glitch the output + * else port->PIO_CODR = mask; \ + * + * port->PIO_IDR = mask; \ // disable interrupt + * + * uint32_t pin_config = G2_g_APinDescription[IO].ulPinConfiguration; \ + * if (pin_config & PIO_PULLUP) pPio->PIO_PUER = mask; \ // enable pullup if necessary + * else pPio->PIO_PUDR = mask; \ + * + * if (pin_config & PIO_OPENDRAIN) port->PIO_MDER = mask; \ // Enable multi-drive if necessary + * else port->PIO_MDDR = mask; \ + * + * port->PIO_PER = mask; \ + * port->PIO_OER = mask; \ // set to output + * + * g_pinStatus[IO] = (g_pinStatus[IO] & 0xF0) | PIN_STATUS_DIGITAL_OUTPUT; \ + * }while(0) + */ + +#else + + // Set pin as input + #define _SET_INPUT(IO) do{ \ + pmc_enable_periph_clk(g_APinDescription[IO].ulPeripheralId); \ + PIO_Configure(digitalPinToPort(IO), PIO_INPUT, digitalPinToBitMask(IO), 0); \ + }while(0) + + // Set pin as output + #define _SET_OUTPUT(IO) do{ \ + pmc_enable_periph_clk(g_APinDescription[IO].ulPeripheralId); \ + PIO_Configure(digitalPinToPort(IO), _READ(IO) ? PIO_OUTPUT_1 : PIO_OUTPUT_0, digitalPinToBitMask(IO), g_APinDescription[IO].ulPinConfiguration); \ + g_pinStatus[IO] = (g_pinStatus[IO] & 0xF0) | PIN_STATUS_DIGITAL_OUTPUT; \ + }while(0) +#endif + +// Set pin as input with pullup mode +#define _PULLUP(IO,V) pinMode(IO, (V) ? INPUT_PULLUP : INPUT) + +// Read a pin (wrapper) +#define READ(IO) _READ(IO) + +// Write to a pin (wrapper) +#define WRITE(IO,V) _WRITE(IO,V) + +// Toggle a pin (wrapper) +#define TOGGLE(IO) _TOGGLE(IO) + +// Set pin as input (wrapper) +#define SET_INPUT(IO) _SET_INPUT(IO) +// Set pin as input with pullup (wrapper) +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) +// Set pin as input with pulldown (substitution) +#define SET_INPUT_PULLDOWN SET_INPUT + +// Set pin as output (wrapper) - reads the pin and sets the output to that value +#define SET_OUTPUT(IO) _SET_OUTPUT(IO) +// Set pin as PWM +#define SET_PWM SET_OUTPUT + +// Check if pin is an input +#define IS_INPUT(IO) ((digitalPinToPort(IO)->PIO_OSR & digitalPinToBitMask(IO)) == 0) +// Check if pin is an output +#define IS_OUTPUT(IO) ((digitalPinToPort(IO)->PIO_OSR & digitalPinToBitMask(IO)) != 0) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +/** + * Ports and functions + * Added as necessary or if I feel like it- not a comprehensive list! + */ + +// UART +#define RXD DIO0 +#define TXD DIO1 + +// TWI (I2C) +#define SCL DIO21 +#define SDA DIO20 + +/** + * pins + */ + +#define DIO0_PIN 8 +#define DIO0_WPORT PIOA + +#define DIO1_PIN 9 +#define DIO1_WPORT PIOA + +#define DIO2_PIN 25 +#define DIO2_WPORT PIOB + +#define DIO3_PIN 28 +#define DIO3_WPORT PIOC + +#define DIO4_PIN 26 +#define DIO4_WPORT PIOC + +#define DIO5_PIN 25 +#define DIO5_WPORT PIOC + +#define DIO6_PIN 24 +#define DIO6_WPORT PIOC + +#define DIO7_PIN 23 +#define DIO7_WPORT PIOC + +#define DIO8_PIN 22 +#define DIO8_WPORT PIOC + +#define DIO9_PIN 21 +#define DIO9_WPORT PIOC + +#define DIO10_PIN 29 +#define DIO10_WPORT PIOC + +#define DIO11_PIN 7 +#define DIO11_WPORT PIOD + +#define DIO12_PIN 8 +#define DIO12_WPORT PIOD + +#define DIO13_PIN 27 +#define DIO13_WPORT PIOB + +#define DIO14_PIN 4 +#define DIO14_WPORT PIOD + +#define DIO15_PIN 5 +#define DIO15_WPORT PIOD + +#define DIO16_PIN 13 +#define DIO16_WPORT PIOA + +#define DIO17_PIN 12 +#define DIO17_WPORT PIOA + +#define DIO18_PIN 11 +#define DIO18_WPORT PIOA + +#define DIO19_PIN 10 +#define DIO19_WPORT PIOA + +#define DIO20_PIN 12 +#define DIO20_WPORT PIOB + +#define DIO21_PIN 13 +#define DIO21_WPORT PIOB + +#define DIO22_PIN 26 +#define DIO22_WPORT PIOB + +#define DIO23_PIN 14 +#define DIO23_WPORT PIOA + +#define DIO24_PIN 15 +#define DIO24_WPORT PIOA + +#define DIO25_PIN 0 +#define DIO25_WPORT PIOD + +#define DIO26_PIN 1 +#define DIO26_WPORT PIOD + +#define DIO27_PIN 2 +#define DIO27_WPORT PIOD + +#define DIO28_PIN 3 +#define DIO28_WPORT PIOD + +#define DIO29_PIN 6 +#define DIO29_WPORT PIOD + +#define DIO30_PIN 9 +#define DIO30_WPORT PIOD + +#define DIO31_PIN 7 +#define DIO31_WPORT PIOA + +#define DIO32_PIN 10 +#define DIO32_WPORT PIOD + +#define DIO33_PIN 1 +#define DIO33_WPORT PIOC + +#if !MB(PRINTRBOARD_G2) // normal DUE pin mapping + + #define DIO34_PIN 2 + #define DIO34_WPORT PIOC + + #define DIO35_PIN 3 + #define DIO35_WPORT PIOC + + #define DIO36_PIN 4 + #define DIO36_WPORT PIOC + + #define DIO37_PIN 5 + #define DIO37_WPORT PIOC + + #define DIO38_PIN 6 + #define DIO38_WPORT PIOC + + #define DIO39_PIN 7 + #define DIO39_WPORT PIOC + + #define DIO40_PIN 8 + #define DIO40_WPORT PIOC + + #define DIO41_PIN 9 + #define DIO41_WPORT PIOC + +#endif // !PRINTRBOARD_G2 + +#define DIO42_PIN 19 +#define DIO42_WPORT PIOA + +#define DIO43_PIN 20 +#define DIO43_WPORT PIOA + +#define DIO44_PIN 19 +#define DIO44_WPORT PIOC + +#define DIO45_PIN 18 +#define DIO45_WPORT PIOC + +#define DIO46_PIN 17 +#define DIO46_WPORT PIOC + +#define DIO47_PIN 16 +#define DIO47_WPORT PIOC + +#define DIO48_PIN 15 +#define DIO48_WPORT PIOC + +#define DIO49_PIN 14 +#define DIO49_WPORT PIOC + +#define DIO50_PIN 13 +#define DIO50_WPORT PIOC + +#define DIO51_PIN 12 +#define DIO51_WPORT PIOC + +#define DIO52_PIN 21 +#define DIO52_WPORT PIOB + +#define DIO53_PIN 14 +#define DIO53_WPORT PIOB + +#define DIO54_PIN 16 +#define DIO54_WPORT PIOA + +#define DIO55_PIN 24 +#define DIO55_WPORT PIOA + +#define DIO56_PIN 23 +#define DIO56_WPORT PIOA + +#define DIO57_PIN 22 +#define DIO57_WPORT PIOA + +#define DIO58_PIN 6 +#define DIO58_WPORT PIOA + +#define DIO59_PIN 4 +#define DIO59_WPORT PIOA + +#define DIO60_PIN 3 +#define DIO60_WPORT PIOA + +#define DIO61_PIN 2 +#define DIO61_WPORT PIOA + +#define DIO62_PIN 17 +#define DIO62_WPORT PIOB + +#define DIO63_PIN 18 +#define DIO63_WPORT PIOB + +#define DIO64_PIN 19 +#define DIO64_WPORT PIOB + +#define DIO65_PIN 20 +#define DIO65_WPORT PIOB + +#define DIO66_PIN 15 +#define DIO66_WPORT PIOB + +#define DIO67_PIN 16 +#define DIO67_WPORT PIOB + +#define DIO68_PIN 1 +#define DIO68_WPORT PIOA + +#define DIO69_PIN 0 +#define DIO69_WPORT PIOA + +#define DIO70_PIN 17 +#define DIO70_WPORT PIOA + +#define DIO71_PIN 18 +#define DIO71_WPORT PIOA + +#define DIO72_PIN 30 +#define DIO72_WPORT PIOC + +#define DIO73_PIN 21 +#define DIO73_WPORT PIOA + +#define DIO74_PIN 25 +#define DIO74_WPORT PIOA + +#define DIO75_PIN 26 +#define DIO75_WPORT PIOA + +#define DIO76_PIN 27 +#define DIO76_WPORT PIOA + +#define DIO77_PIN 28 +#define DIO77_WPORT PIOA + +#define DIO78_PIN 23 +#define DIO78_WPORT PIOB + +#define DIO79_PIN 17 +#define DIO79_WPORT PIOA + +#define DIO80_PIN 12 +#define DIO80_WPORT PIOB + +#define DIO81_PIN 8 +#define DIO81_WPORT PIOA + +#define DIO82_PIN 11 +#define DIO82_WPORT PIOA + +#define DIO83_PIN 13 +#define DIO83_WPORT PIOA + +#define DIO84_PIN 4 +#define DIO84_WPORT PIOD + +#define DIO85_PIN 11 +#define DIO85_WPORT PIOB + +#define DIO86_PIN 21 +#define DIO86_WPORT PIOB + +#define DIO87_PIN 29 +#define DIO87_WPORT PIOA + +#define DIO88_PIN 15 +#define DIO88_WPORT PIOB + +#define DIO89_PIN 14 +#define DIO89_WPORT PIOB + +#define DIO90_PIN 1 +#define DIO90_WPORT PIOA + +#define DIO91_PIN 15 +#define DIO91_WPORT PIOB + +#ifdef ARDUINO_SAM_ARCHIM + + #define DIO92_PIN 11 + #define DIO92_WPORT PIOC + + #define DIO93_PIN 2 + #define DIO93_WPORT PIOB + + #define DIO94_PIN 1 + #define DIO94_WPORT PIOB + + #define DIO95_PIN 0 + #define DIO95_WPORT PIOB + + #define DIO96_PIN 10 + #define DIO96_WPORT PIOC + + #define DIO97_PIN 24 + #define DIO97_WPORT PIOB + + #define DIO98_PIN 7 + #define DIO98_WPORT PIOB + + #define DIO99_PIN 6 + #define DIO99_WPORT PIOB + + #define DIO100_PIN 8 + #define DIO100_WPORT PIOB + + #define DIO101_PIN 5 + #define DIO101_WPORT PIOB + + #define DIO102_PIN 4 + #define DIO102_WPORT PIOB + + #define DIO103_PIN 3 + #define DIO103_WPORT PIOB + + #define DIO104_PIN 20 + #define DIO104_WPORT PIOC + + #define DIO105_PIN 22 + #define DIO105_WPORT PIOB + + #define DIO106_PIN 27 + #define DIO106_WPORT PIOC + + #define DIO107_PIN 10 + #define DIO107_WPORT PIOB + + #define DIO108_PIN 9 + #define DIO108_WPORT PIOB + +#else // !ARDUINO_SAM_ARCHIM + + #define DIO92_PIN 5 + #define DIO92_WPORT PIOA + + #define DIO93_PIN 12 + #define DIO93_WPORT PIOB + + #define DIO94_PIN 22 + #define DIO94_WPORT PIOB + + #define DIO95_PIN 23 + #define DIO95_WPORT PIOB + + #define DIO96_PIN 24 + #define DIO96_WPORT PIOB + + #define DIO97_PIN 20 + #define DIO97_WPORT PIOC + + #define DIO98_PIN 27 + #define DIO98_WPORT PIOC + + #define DIO99_PIN 10 + #define DIO99_WPORT PIOC + + #define DIO100_PIN 11 + #define DIO100_WPORT PIOC + +#endif // !ARDUINO_SAM_ARCHIM diff --git a/src/HAL/DUE/fastio/G2_PWM.cpp b/src/HAL/DUE/fastio/G2_PWM.cpp new file mode 100644 index 0000000..800915f --- /dev/null +++ b/src/HAL/DUE/fastio/G2_PWM.cpp @@ -0,0 +1,206 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * The PWM module is only used to generate interrupts at specified times. It + * is NOT used to directly toggle pins. The ISR writes to the pin assigned to + * that interrupt. + * + * All PWMs use the same repetition rate. The G2 needs about 10kHz min in order to + * not have obvious ripple on the Vref signals. + * + * The data structures are setup to minimize the computation done by the ISR which + * minimizes ISR execution time. Execution times are 0.8 to 1.1 microseconds. + * + * FIve PWM interrupt sources are used. Channel 0 sets the base period. All Vref + * signals are set active when this counter overflows and resets to zero. The compare + * values in channels 1-4 are set to give the desired duty cycle for that Vref pin. + * When counter 0 matches the compare value then that channel generates an interrupt. + * The ISR checks the source of the interrupt and sets the corresponding pin inactive. + * + * Some jitter in the Vref signal is OK so the interrupt priority is left at its default value. + */ + +#include "../../../inc/MarlinConfig.h" + +#if MB(PRINTRBOARD_G2) + +#include "G2_PWM.h" + +#if PIN_EXISTS(MOTOR_CURRENT_PWM_X) + #define G2_PWM_X 1 +#else + #define G2_PWM_X 0 +#endif +#if PIN_EXISTS(MOTOR_CURRENT_PWM_Y) + #define G2_PWM_Y 1 +#else + #define G2_PWM_Y 0 +#endif +#if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + #define G2_PWM_Z 1 +#else + #define G2_PWM_Z 0 +#endif +#if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + #define G2_PWM_E 1 +#else + #define G2_PWM_E 0 +#endif +#define G2_MASK_X(V) (G2_PWM_X * (V)) +#define G2_MASK_Y(V) (G2_PWM_Y * (V)) +#define G2_MASK_Z(V) (G2_PWM_Z * (V)) +#define G2_MASK_E(V) (G2_PWM_E * (V)) + +volatile uint32_t *SODR_A = &PIOA->PIO_SODR, + *SODR_B = &PIOB->PIO_SODR, + *CODR_A = &PIOA->PIO_CODR, + *CODR_B = &PIOB->PIO_CODR; + +PWM_map ISR_table[NUM_PWMS] = PWM_MAP_INIT; + +void Stepper::digipot_init() { + + #if PIN_EXISTS(MOTOR_CURRENT_PWM_X) + OUT_WRITE(MOTOR_CURRENT_PWM_X_PIN, 0); // init pins + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Y) + OUT_WRITE(MOTOR_CURRENT_PWM_Y_PIN, 0); + #endif + #if G2_PWM_Z + OUT_WRITE(MOTOR_CURRENT_PWM_Z_PIN, 0); + #endif + #if G2_PWM_E + OUT_WRITE(MOTOR_CURRENT_PWM_E_PIN, 0); + #endif + + #define WPKEY (0x50574D << 8) // “PWM” in ASCII + #define WPCMD_DIS_SW 0 // command to disable Write Protect SW + #define WPRG_ALL (PWM_WPCR_WPRG0 | PWM_WPCR_WPRG1 | PWM_WPCR_WPRG2 | PWM_WPCR_WPRG3 | PWM_WPCR_WPRG4 | PWM_WPCR_WPRG5) // all Write Protect Groups + + #define PWM_CLOCK_F F_CPU / 1000000UL // set clock to 1MHz + + PMC->PMC_PCER1 = PMC_PCER1_PID36; // enable PWM controller clock (disabled on power up) + + PWM->PWM_WPCR = WPKEY | WPRG_ALL | WPCMD_DIS_SW; // enable setting of all PWM registers + PWM->PWM_CLK = PWM_CLOCK_F; // enable CLK_A and set it to 1MHz, leave CLK_B disabled + PWM->PWM_CH_NUM[0].PWM_CMR = 0b1011; // set channel 0 to Clock A input & to left aligned + if (G2_PWM_X) PWM->PWM_CH_NUM[1].PWM_CMR = 0b1011; // set channel 1 to Clock A input & to left aligned + if (G2_PWM_Y) PWM->PWM_CH_NUM[2].PWM_CMR = 0b1011; // set channel 2 to Clock A input & to left aligned + if (G2_PWM_Z) PWM->PWM_CH_NUM[3].PWM_CMR = 0b1011; // set channel 3 to Clock A input & to left aligned + if (G2_PWM_E) PWM->PWM_CH_NUM[4].PWM_CMR = 0b1011; // set channel 4 to Clock A input & to left aligned + + PWM->PWM_CH_NUM[0].PWM_CPRD = PWM_PERIOD_US; // set channel 0 Period + + PWM->PWM_IER2 = PWM_IER1_CHID0; // generate interrupt when counter0 overflows + PWM->PWM_IER2 = PWM_IER2_CMPM0 + | G2_MASK_X(PWM_IER2_CMPM1) + | G2_MASK_Y(PWM_IER2_CMPM2) + | G2_MASK_Z(PWM_IER2_CMPM3) + | G2_MASK_E(PWM_IER2_CMPM4) + ; // generate interrupt on compare event + + if (G2_PWM_X) PWM->PWM_CMP[1].PWM_CMPV = 0x010000000LL | G2_VREF_COUNT(G2_VREF(motor_current_setting[0])); // interrupt when counter0 == CMPV - used to set Motor 1 PWM inactive + if (G2_PWM_Y) PWM->PWM_CMP[2].PWM_CMPV = 0x010000000LL | G2_VREF_COUNT(G2_VREF(motor_current_setting[0])); // interrupt when counter0 == CMPV - used to set Motor 2 PWM inactive + if (G2_PWM_Z) PWM->PWM_CMP[3].PWM_CMPV = 0x010000000LL | G2_VREF_COUNT(G2_VREF(motor_current_setting[1])); // interrupt when counter0 == CMPV - used to set Motor 3 PWM inactive + if (G2_PWM_E) PWM->PWM_CMP[4].PWM_CMPV = 0x010000000LL | G2_VREF_COUNT(G2_VREF(motor_current_setting[2])); // interrupt when counter0 == CMPV - used to set Motor 4 PWM inactive + + if (G2_PWM_X) PWM->PWM_CMP[1].PWM_CMPM = 0x0001; // enable compare event + if (G2_PWM_Y) PWM->PWM_CMP[2].PWM_CMPM = 0x0001; // enable compare event + if (G2_PWM_Z) PWM->PWM_CMP[3].PWM_CMPM = 0x0001; // enable compare event + if (G2_PWM_E) PWM->PWM_CMP[4].PWM_CMPM = 0x0001; // enable compare event + + PWM->PWM_SCM = PWM_SCM_UPDM_MODE0 | PWM_SCM_SYNC0 + | G2_MASK_X(PWM_SCM_SYNC1) + | G2_MASK_Y(PWM_SCM_SYNC2) + | G2_MASK_Z(PWM_SCM_SYNC3) + | G2_MASK_E(PWM_SCM_SYNC4) + ; // sync 1-4 with 0, use mode 0 for updates + + PWM->PWM_ENA = PWM_ENA_CHID0 + | G2_MASK_X(PWM_ENA_CHID1) + | G2_MASK_Y(PWM_ENA_CHID2) + | G2_MASK_Z(PWM_ENA_CHID3) + | G2_MASK_E(PWM_ENA_CHID4) + ; // enable channels used by G2 + + PWM->PWM_IER1 = PWM_IER1_CHID0 + | G2_MASK_X(PWM_IER1_CHID1) + | G2_MASK_Y(PWM_IER1_CHID2) + | G2_MASK_Z(PWM_IER1_CHID3) + | G2_MASK_E(PWM_IER1_CHID4) + ; // enable interrupts for channels used by G2 + + NVIC_EnableIRQ(PWM_IRQn); // Enable interrupt handler + NVIC_SetPriority(PWM_IRQn, NVIC_EncodePriority(0, 10, 0)); // normal priority for PWM module (can stand some jitter on the Vref signals) +} + +void Stepper::set_digipot_current(const uint8_t driver, const int16_t current) { + + if (!(PWM->PWM_CH_NUM[0].PWM_CPRD == PWM_PERIOD_US)) digipot_init(); // Init PWM system if needed + + switch (driver) { + case 0: + if (G2_PWM_X) PWM->PWM_CMP[1].PWM_CMPVUPD = 0x010000000LL | G2_VREF_COUNT(G2_VREF(current)); // update X & Y + if (G2_PWM_Y) PWM->PWM_CMP[2].PWM_CMPVUPD = 0x010000000LL | G2_VREF_COUNT(G2_VREF(current)); + if (G2_PWM_X) PWM->PWM_CMP[1].PWM_CMPMUPD = 0x0001; // enable compare event + if (G2_PWM_Y) PWM->PWM_CMP[2].PWM_CMPMUPD = 0x0001; // enable compare event + if (G2_PWM_X || G2_PWM_Y) PWM->PWM_SCUC = PWM_SCUC_UPDULOCK; // tell the PWM controller to update the values on the next cycle + break; + case 1: + if (G2_PWM_Z) { + PWM->PWM_CMP[3].PWM_CMPVUPD = 0x010000000LL | G2_VREF_COUNT(G2_VREF(current)); // update Z + PWM->PWM_CMP[3].PWM_CMPMUPD = 0x0001; // enable compare event + PWM->PWM_SCUC = PWM_SCUC_UPDULOCK; // tell the PWM controller to update the values on the next cycle + } + break; + default: + if (G2_PWM_E) { + PWM->PWM_CMP[4].PWM_CMPVUPD = 0x010000000LL | G2_VREF_COUNT(G2_VREF(current)); // update E + PWM->PWM_CMP[4].PWM_CMPMUPD = 0x0001; // enable compare event + PWM->PWM_SCUC = PWM_SCUC_UPDULOCK; // tell the PWM controller to update the values on the next cycle + } + break; + } +} + +volatile uint32_t PWM_ISR1_STATUS, PWM_ISR2_STATUS; + +void PWM_Handler() { + PWM_ISR1_STATUS = PWM->PWM_ISR1; + PWM_ISR2_STATUS = PWM->PWM_ISR2; + if (PWM_ISR1_STATUS & PWM_IER1_CHID0) { // CHAN_0 interrupt + if (G2_PWM_X) *ISR_table[0].set_register = ISR_table[0].write_mask; // set X to active + if (G2_PWM_Y) *ISR_table[1].set_register = ISR_table[1].write_mask; // set Y to active + if (G2_PWM_Z) *ISR_table[2].set_register = ISR_table[2].write_mask; // set Z to active + if (G2_PWM_E) *ISR_table[3].set_register = ISR_table[3].write_mask; // set E to active + } + else { + if (G2_PWM_X && (PWM_ISR2_STATUS & PWM_IER2_CMPM1)) *ISR_table[0].clr_register = ISR_table[0].write_mask; // set X to inactive + if (G2_PWM_Y && (PWM_ISR2_STATUS & PWM_IER2_CMPM2)) *ISR_table[1].clr_register = ISR_table[1].write_mask; // set Y to inactive + if (G2_PWM_Z && (PWM_ISR2_STATUS & PWM_IER2_CMPM3)) *ISR_table[2].clr_register = ISR_table[2].write_mask; // set Z to inactive + if (G2_PWM_E && (PWM_ISR2_STATUS & PWM_IER2_CMPM4)) *ISR_table[3].clr_register = ISR_table[3].write_mask; // set E to inactive + } + return; +} + +#endif // PRINTRBOARD_G2 diff --git a/src/HAL/DUE/fastio/G2_PWM.h b/src/HAL/DUE/fastio/G2_PWM.h new file mode 100644 index 0000000..dc4edff --- /dev/null +++ b/src/HAL/DUE/fastio/G2_PWM.h @@ -0,0 +1,78 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * This module is stripped down version of the LPC1768_PWM.h file from + * PR #7500. It is hardwired for the PRINTRBOARD_G2 Motor Current needs. + */ + +#include "../../../inc/MarlinConfigPre.h" +#include "../../../module/stepper.h" +//C:\Users\bobku\Documents\GitHub\Marlin-Bob-2\Marlin\src\module\stepper.h +//C:\Users\bobku\Documents\GitHub\Marlin-Bob-2\Marlin\src\HAL\HAL_DUE\G2_PWM.h + +#define PWM_PERIOD_US 100 // base repetition rate in micro seconds + +typedef struct { // holds the data needed by the ISR to control the Vref pin + volatile uint32_t* set_register; + volatile uint32_t* clr_register; + uint32_t write_mask; +} PWM_map; + +#define G2_VREF(I) (uint32_t)(I * 5 * 0.15) // desired Vref * 1000 (scaled so don't loose accuracy in next step) + +#define G2_VREF_COUNT(Q) (uint32_t)map(constrain(Q, 500, 3.3 * 1000), 0, 3.3 * 1000, 0, PWM_PERIOD_US) // under 500 the results are very non-linear + +extern volatile uint32_t *SODR_A, *SODR_B, *CODR_A, *CODR_B; + +#define _PIN(IO) (DIO ## IO ## _PIN) + +#define PWM_MAP_INIT_ROW(IO,ZZ) { ZZ == 'A' ? SODR_A : SODR_B, ZZ == 'A' ? CODR_A : CODR_B, 1 << _PIN(IO) } + + +#define PWM_MAP_INIT { PWM_MAP_INIT_ROW(MOTOR_CURRENT_PWM_X_PIN, 'B'), \ + PWM_MAP_INIT_ROW(MOTOR_CURRENT_PWM_Y_PIN, 'B'), \ + PWM_MAP_INIT_ROW(MOTOR_CURRENT_PWM_Z_PIN, 'B'), \ + PWM_MAP_INIT_ROW(MOTOR_CURRENT_PWM_E_PIN, 'A'), \ + }; + +#define NUM_PWMS 4 + +extern PWM_map ISR_table[NUM_PWMS]; + +extern uint32_t motor_current_setting[3]; + +#define IR_BIT(p) (WITHIN(p, 0, 3) ? (p) : (p) + 4) +#define COPY_ACTIVE_TABLE() do{ LOOP_L_N(i, 6) work_table[i] = active_table[i]; }while(0) + +#define PWM_MR0 19999 // base repetition rate minus one count - 20mS +#define PWM_PR 24 // prescaler value - prescaler divide by 24 + 1 - 1 MHz output +#define PWM_PCLKSEL0 0x00 // select clock source for prescaler - defaults to 25MHz on power up + // 0: 25MHz, 1: 100MHz, 2: 50MHz, 3: 12.5MHZ to PWM1 prescaler +#define MR0_MARGIN 200 // if channel value too close to MR0 the system locks up + +extern bool PWM_table_swap; // flag to tell the ISR that the tables have been swapped + +#define HAL_G2_PWM_ISR void PWM_Handler() + +extern volatile uint32_t PWM_ISR1_STATUS, PWM_ISR2_STATUS; diff --git a/src/HAL/DUE/fastio/G2_pins.h b/src/HAL/DUE/fastio/G2_pins.h new file mode 100644 index 0000000..80c87bd --- /dev/null +++ b/src/HAL/DUE/fastio/G2_pins.h @@ -0,0 +1,278 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +/** + * This file contains the custom port/pin definitions for the PRINTRBOARD_G2 + * motherboard. This motherboard uses the SAM3X8C which is a subset of the + * SAM3X8E used in the DUE board. It uses port/pin pairs that are not + * available using the DUE definitions. + * + * The first part is a copy of the pin descriptions in the + * "variants\arduino_due_x\variant.cpp" file but with pins 34-41 replaced by + * the G2 pins. + * + * The second part is the FASTIO port/pin definitions. + * + * THESE PINS CAN ONLY BE ACCESSED VIA FASTIO COMMANDS. + */ + +/* + Copyright (c) 2011 Arduino. All right reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library 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 Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +typedef struct _G2_PinDescription { + Pio* pPort; + uint32_t ulPin; + uint32_t ulPeripheralId; + EPioType ulPinType; + uint32_t ulPinConfiguration; + uint32_t ulPinAttribute; + EAnalogChannel ulAnalogChannel; /* Analog pin in the Arduino context (label on the board) */ + EAnalogChannel ulADCChannelNumber; /* ADC Channel number in the SAM device */ + EPWMChannel ulPWMChannel; + ETCChannel ulTCChannel; +} G2_PinDescription; + +/** + * This section is a copy of the pin descriptions in the "variants\arduino_due_x\variant.cpp" file + * with pins 34-41 replaced by the G2 pins. + */ + +/** + * Pins descriptions + */ +const G2_PinDescription G2_g_APinDescription[] = { + // 0 .. 53 - Digital pins + // ---------------------- + // 0/1 - UART (Serial) + { PIOA, PIO_PA8A_URXD, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // URXD + { PIOA, PIO_PA9A_UTXD, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // UTXD + + // 2 + { PIOB, PIO_PB25B_TIOA0, ID_PIOB, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC0_CHA0 }, // TIOA0 + { PIOC, PIO_PC28B_TIOA7, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHA7 }, // TIOA7 + { PIOC, PIO_PC26B_TIOB6, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHB6 }, // TIOB6 + + // 5 + { PIOC, PIO_PC25B_TIOA6, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHA6 }, // TIOA6 + { PIOC, PIO_PC24B_PWML7, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM), NO_ADC, NO_ADC, PWM_CH7, NOT_ON_TIMER }, // PWML7 + { PIOC, PIO_PC23B_PWML6, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM), NO_ADC, NO_ADC, PWM_CH6, NOT_ON_TIMER }, // PWML6 + { PIOC, PIO_PC22B_PWML5, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM), NO_ADC, NO_ADC, PWM_CH5, NOT_ON_TIMER }, // PWML5 + { PIOC, PIO_PC21B_PWML4, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_PWM), NO_ADC, NO_ADC, PWM_CH4, NOT_ON_TIMER }, // PWML4 + // 10 + { PIOC, PIO_PC29B_TIOB7, ID_PIOC, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHB7 }, // TIOB7 + { PIOD, PIO_PD7B_TIOA8, ID_PIOD, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHA8 }, // TIOA8 + { PIOD, PIO_PD8B_TIOB8, ID_PIOD, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC2_CHB8 }, // TIOB8 + + // 13 - AMBER LED + { PIOB, PIO_PB27B_TIOB0, ID_PIOB, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_TIMER), NO_ADC, NO_ADC, NOT_ON_PWM, TC0_CHB0 }, // TIOB0 + + // 14/15 - USART3 (Serial3) + { PIOD, PIO_PD4B_TXD3, ID_PIOD, PIO_PERIPH_B, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TXD3 + { PIOD, PIO_PD5B_RXD3, ID_PIOD, PIO_PERIPH_B, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // RXD3 + + // 16/17 - USART1 (Serial2) + { PIOA, PIO_PA13A_TXD1, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TXD1 + { PIOA, PIO_PA12A_RXD1, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // RXD1 + + // 18/19 - USART0 (Serial1) + { PIOA, PIO_PA11A_TXD0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TXD0 + { PIOA, PIO_PA10A_RXD0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // RXD0 + + // 20/21 - TWI1 + { PIOB, PIO_PB12A_TWD1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TWD1 - SDA0 + { PIOB, PIO_PB13A_TWCK1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TWCK1 - SCL0 + + // 22 + { PIOB, PIO_PB26, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 22 + { PIOA, PIO_PA14, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 23 + { PIOA, PIO_PA15, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 24 + { PIOD, PIO_PD0, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 25 + + // 26 + { PIOD, PIO_PD1, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 26 + { PIOD, PIO_PD2, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 27 + { PIOD, PIO_PD3, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 28 + { PIOD, PIO_PD6, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 29 + + // 30 + { PIOD, PIO_PD9, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 30 + { PIOA, PIO_PA7, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 31 + { PIOD, PIO_PD10, ID_PIOD, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 32 + { PIOC, PIO_PC1, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 33 + + // 34 + + // start of custom pins + { PIOA, PIO_PA29, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 34 Y_STEP_PIN + { PIOB, PIO_PB1, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 35 Y_DIR_PIN + { PIOB, PIO_PB0, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 36 Y_ENABLE_PIN + { PIOB, PIO_PB22, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 37 E0_ENABLE_PIN + { PIOB, PIO_PB11, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 38 E0_MS1_PIN + { PIOB, PIO_PB10, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 39 E0_MS3_PIN + { PIOA, PIO_PA5, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 40 HEATER_0_PIN + { PIOB, PIO_PB24, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 41 HEATER_BED_PIN + // end of custom pins + + // 42 + { PIOA, PIO_PA19, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 42 + { PIOA, PIO_PA20, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 43 + { PIOC, PIO_PC19, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 44 + { PIOC, PIO_PC18, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 45 + + // 46 + { PIOC, PIO_PC17, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 46 + { PIOC, PIO_PC16, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 47 + { PIOC, PIO_PC15, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 48 + { PIOC, PIO_PC14, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 49 + + // 50 + { PIOC, PIO_PC13, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 50 + { PIOC, PIO_PC12, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 51 + { PIOB, PIO_PB21, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 52 + { PIOB, PIO_PB14, ID_PIOB, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // PIN 53 + + + // 54 .. 65 - Analog pins + // ---------------------- + { PIOA, PIO_PA16X1_AD7, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC0, ADC7, NOT_ON_PWM, NOT_ON_TIMER }, // AD0 + { PIOA, PIO_PA24X1_AD6, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC1, ADC6, NOT_ON_PWM, NOT_ON_TIMER }, // AD1 + { PIOA, PIO_PA23X1_AD5, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC2, ADC5, NOT_ON_PWM, NOT_ON_TIMER }, // AD2 + { PIOA, PIO_PA22X1_AD4, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC3, ADC4, NOT_ON_PWM, NOT_ON_TIMER }, // AD3 + // 58 + { PIOA, PIO_PA6X1_AD3, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC4, ADC3, NOT_ON_PWM, TC0_CHB2 }, // AD4 + { PIOA, PIO_PA4X1_AD2, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC5, ADC2, NOT_ON_PWM, NOT_ON_TIMER }, // AD5 + { PIOA, PIO_PA3X1_AD1, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC6, ADC1, NOT_ON_PWM, TC0_CHB1 }, // AD6 + { PIOA, PIO_PA2X1_AD0, ID_PIOA, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC7, ADC0, NOT_ON_PWM, TC0_CHA1 }, // AD7 + // 62 + { PIOB, PIO_PB17X1_AD10, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC8, ADC10, NOT_ON_PWM, NOT_ON_TIMER }, // AD8 + { PIOB, PIO_PB18X1_AD11, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC9, ADC11, NOT_ON_PWM, NOT_ON_TIMER }, // AD9 + { PIOB, PIO_PB19X1_AD12, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC10, ADC12, NOT_ON_PWM, NOT_ON_TIMER }, // AD10 + { PIOB, PIO_PB20X1_AD13, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC11, ADC13, NOT_ON_PWM, NOT_ON_TIMER }, // AD11 + + // 66/67 - DAC0/DAC1 + { PIOB, PIO_PB15X1_DAC0, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC12, DA0, NOT_ON_PWM, NOT_ON_TIMER }, // DAC0 + { PIOB, PIO_PB16X1_DAC1, ID_PIOB, PIO_INPUT, PIO_DEFAULT, PIN_ATTR_ANALOG, ADC13, DA1, NOT_ON_PWM, NOT_ON_TIMER }, // DAC1 + + // 68/69 - CANRX0/CANTX0 + { PIOA, PIO_PA1A_CANRX0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, ADC14, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // CANRX + { PIOA, PIO_PA0A_CANTX0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, ADC15, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // CANTX + + // 70/71 - TWI0 + { PIOA, PIO_PA17A_TWD0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TWD0 - SDA1 + { PIOA, PIO_PA18A_TWCK0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // TWCK0 - SCL1 + + // 72/73 - LEDs + { PIOC, PIO_PC30, ID_PIOC, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // LED AMBER RXL + { PIOA, PIO_PA21, ID_PIOA, PIO_OUTPUT_0, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // LED AMBER TXL + + // 74/75/76 - SPI + { PIOA, PIO_PA25A_SPI0_MISO,ID_PIOA,PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // MISO + { PIOA, PIO_PA26A_SPI0_MOSI,ID_PIOA,PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // MOSI + { PIOA, PIO_PA27A_SPI0_SPCK,ID_PIOA,PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // SPCK + + // 77 - SPI CS0 + { PIOA, PIO_PA28A_SPI0_NPCS0,ID_PIOA,PIO_PERIPH_A,PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // NPCS0 + + // 78 - SPI CS3 (unconnected) + { PIOB, PIO_PB23B_SPI0_NPCS3,ID_PIOB,PIO_PERIPH_B,PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // NPCS3 + + // 79 .. 84 - "All pins" masks + + // 79 - TWI0 all pins + { PIOA, PIO_PA17A_TWD0|PIO_PA18A_TWCK0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 80 - TWI1 all pins + { PIOB, PIO_PB12A_TWD1|PIO_PB13A_TWCK1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 81 - UART (Serial) all pins + { PIOA, PIO_PA8A_URXD|PIO_PA9A_UTXD, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 82 - USART0 (Serial1) all pins + { PIOA, PIO_PA11A_TXD0|PIO_PA10A_RXD0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 83 - USART1 (Serial2) all pins + { PIOA, PIO_PA13A_TXD1|PIO_PA12A_RXD1, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 84 - USART3 (Serial3) all pins + { PIOD, PIO_PD4B_TXD3|PIO_PD5B_RXD3, ID_PIOD, PIO_PERIPH_B, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + + // 85 - USB + { PIOB, PIO_PB11A_UOTGID|PIO_PB10A_UOTGVBOF, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL,NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // ID - VBOF + + // 86 - SPI CS2 + { PIOB, PIO_PB21B_SPI0_NPCS2, ID_PIOB, PIO_PERIPH_B, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // NPCS2 + + // 87 - SPI CS1 + { PIOA, PIO_PA29A_SPI0_NPCS1, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // NPCS1 + + // 88/89 - CANRX1/CANTX1 (same physical pin for 66/53) + { PIOB, PIO_PB15A_CANRX1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // CANRX1 + { PIOB, PIO_PB14A_CANTX1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, PIN_ATTR_DIGITAL, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, // CANTX1 + + // 90 .. 91 - "All CAN pins" masks + // 90 - CAN0 all pins + { PIOA, PIO_PA1A_CANRX0|PIO_PA0A_CANTX0, ID_PIOA, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + // 91 - CAN1 all pins + { PIOB, PIO_PB15A_CANRX1|PIO_PB14A_CANTX1, ID_PIOB, PIO_PERIPH_A, PIO_DEFAULT, (PIN_ATTR_DIGITAL|PIN_ATTR_COMBO), NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER }, + + // END + { nullptr, 0, 0, PIO_NOT_A_PIN, PIO_DEFAULT, 0, NO_ADC, NO_ADC, NOT_ON_PWM, NOT_ON_TIMER } +}; + +// This section replaces the FASTIO definitions of pins 34-41 + +#define DIO34_PIN 29 +#define DIO34_WPORT PIOA // only available via FASTIO // 34 PA29 - Y_STEP_PIN + +#define DIO35_PIN 1 +#define DIO35_WPORT PIOB // only available via FASTIO // 35 PAB1 - Y_DIR_PIN + +#define DIO36_PIN 0 +#define DIO36_WPORT PIOB // only available via FASTIO // 36 PB0 - Y_ENABLE_PIN + +#define DIO37_PIN 22 +#define DIO37_WPORT PIOB // only available via FASTIO // 37 PB22 - E0_ENABLE_PIN + +#define DIO38_PIN 11 +#define DIO38_WPORT PIOB // only available via FASTIO // 38 PB11 - E0_MS1_PIN + +#define DIO39_PIN 10 +#define DIO39_WPORT PIOB // only available via FASTIO // 39 PB10 - E0_MS3_PIN + +#define DIO40_PIN 5 +#define DIO40_WPORT PIOA // only available via FASTIO // 40 PA5 - HEATER_0_PIN + +#define DIO41_PIN 24 +#define DIO41_WPORT PIOB // only available via FASTIO // 41 PB24 - HEATER_BED_PIN diff --git a/src/HAL/DUE/inc/Conditionals_LCD.h b/src/HAL/DUE/inc/Conditionals_LCD.h new file mode 100644 index 0000000..5867414 --- /dev/null +++ b/src/HAL/DUE/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/DUE." +#endif diff --git a/src/HAL/DUE/inc/Conditionals_adv.h b/src/HAL/DUE/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/DUE/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/DUE/inc/Conditionals_post.h b/src/HAL/DUE/inc/Conditionals_post.h new file mode 100644 index 0000000..ce6d3fd --- /dev/null +++ b/src/HAL/DUE/inc/Conditionals_post.h @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define FLASH_EEPROM_EMULATION +#elif EITHER(I2C_EEPROM, SPI_EEPROM) + #define USE_SHARED_EEPROM 1 +#endif diff --git a/src/HAL/DUE/inc/SanityCheck.h b/src/HAL/DUE/inc/SanityCheck.h new file mode 100644 index 0000000..13484f7 --- /dev/null +++ b/src/HAL/DUE/inc/SanityCheck.h @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test Arduino Due specific configuration values for errors at compile-time. + */ + +/** + * Check for common serial pin conflicts + */ +#define CHECK_SERIAL_PIN(N) ( \ + X_STOP_PIN == N || Y_STOP_PIN == N || Z_STOP_PIN == N \ + || X_MIN_PIN == N || Y_MIN_PIN == N || Z_MIN_PIN == N \ + || X_MAX_PIN == N || Y_MAX_PIN == N || Z_MAX_PIN == N \ + || X_STEP_PIN == N || Y_STEP_PIN == N || Z_STEP_PIN == N \ + || X_DIR_PIN == N || Y_DIR_PIN == N || Z_DIR_PIN == N \ + || X_ENA_PIN == N || Y_ENA_PIN == N || Z_ENA_PIN == N \ +) +#if CONF_SERIAL_IS(0) // D0-D1. No known conflicts. +#endif +#if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19)) + #error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board." +#endif +#if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17)) + #error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board." +#endif +#if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15)) + #error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board." +#endif +#undef CHECK_SERIAL_PIN + +/** + * HARDWARE VS. SOFTWARE SPI COMPATIBILITY + * + * DUE selects hardware vs. software SPI depending on whether one of the hardware-controllable SDSS pins is in use. + * + * The hardware SPI controller doesn't allow software SPIs to control any shared pins. + * + * When DUE software SPI is used then Trinamic drivers must use the TMC softSPI. + * + * When DUE hardware SPI is used then a Trinamic driver can use either its hardware SPI or, if there are no shared + * pins, its software SPI. + * + * Usually the hardware SPI pins are only available to the LCD. This makes the DUE hard SPI used at the same time + * as the TMC2130 soft SPI the most common setup. + */ +#define _IS_HW_SPI(P) (defined(TMC_SW_##P) && (TMC_SW_##P == SD_MOSI_PIN || TMC_SW_##P == SD_MISO_PIN || TMC_SW_##P == SD_SCK_PIN)) + +#if ENABLED(SDSUPPORT) && HAS_DRIVER(TMC2130) + #if ENABLED(TMC_USE_SW_SPI) + #if DISABLED(DUE_SOFTWARE_SPI) && (_IS_HW_SPI(MOSI) || _IS_HW_SPI(MISO) || _IS_HW_SPI(SCK)) + #error "DUE hardware SPI is required but is incompatible with TMC2130 software SPI. Either disable TMC_USE_SW_SPI or use separate pins for the two SPIs." + #endif + #elif ENABLED(DUE_SOFTWARE_SPI) + #error "DUE software SPI is required but is incompatible with TMC2130 hardware SPI. Enable TMC_USE_SW_SPI to fix." + #endif +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on DUE." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on the DUE platform." +#endif + +#if USING_PULLDOWNS + #error "PULLDOWN pin mode is not available on DUE boards." +#endif diff --git a/src/HAL/DUE/pinsDebug.h b/src/HAL/DUE/pinsDebug.h new file mode 100644 index 0000000..df1ba41 --- /dev/null +++ b/src/HAL/DUE/pinsDebug.h @@ -0,0 +1,185 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Support routines for Due + */ + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#include "../shared/Marduino.h" + +/** + * Due/Marlin quirks + * + * a) determining the state of a pin + * The Due/Arduino status definitions for the g_pinStatus[pin] array are: + * #define PIN_STATUS_DIGITAL_INPUT_PULLUP (0x01) + * #define PIN_STATUS_DIGITAL_INPUT (0x02) + * #define PIN_STATUS_DIGITAL_OUTPUT (0x03) + * #define PIN_STATUS_ANALOG (0x04) + * #define PIN_STATUS_PWM (0x05) + * #define PIN_STATUS_TIMER (0x06) + * + * These are only valid if the following Due/Arduino provided functions are used: + * analogRead + * analogWrite + * digitalWrite + * pinMode + * + * The FASTIO routines do not touch the g_pinStatus[pin] array. + * + * The net result is that both the g_pinStatus[pin] array and the PIO_OSR register + * needs to be looked at when determining if a pin is an input or an output. + * + * b) Due has only pins 6, 7, 8 & 9 enabled for PWMs. FYI - they run at 1kHz + * + * c) NUM_DIGITAL_PINS does not include the analog pins + * + * d) Pins 0-78 are defined for Due but 78 has a comment of "unconnected!". 78 is + * included just in case. + */ + +#define NUMBER_PINS_TOTAL PINS_COUNT + +#define digitalRead_mod(p) extDigitalRead(p) // AVR digitalRead disabled PWM before it read the pin +#define PRINT_PORT(p) +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%02d"), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital +#define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL ? 1 : 0) +#define DIGITAL_PIN_TO_ANALOG_PIN(p) int(p - analogInputToDigitalPin(0)) +#define IS_ANALOG(P) WITHIN(P, char(analogInputToDigitalPin(0)), char(analogInputToDigitalPin(NUM_ANALOG_INPUTS - 1))) +#define pwm_status(pin) (((g_pinStatus[pin] & 0xF) == PIN_STATUS_PWM) && \ + ((g_APinDescription[pin].ulPinAttribute & PIN_ATTR_PWM) == PIN_ATTR_PWM)) +#define MULTI_NAME_PAD 14 // space needed to be pretty if not first name assigned to a pin + +bool GET_PINMODE(int8_t pin) { // 1: output, 0: input + volatile Pio* port = g_APinDescription[pin].pPort; + uint32_t mask = g_APinDescription[pin].ulPin; + uint8_t pin_status = g_pinStatus[pin] & 0xF; + return ( (pin_status == 0 && (port->PIO_OSR & mask)) + || pin_status == PIN_STATUS_DIGITAL_OUTPUT + || pwm_status(pin)); +} + +void pwm_details(int32_t pin) { + if (pwm_status(pin)) { + uint32_t chan = g_APinDescription[pin].ulPWMChannel; + SERIAL_ECHOPGM("PWM = ", PWM_INTERFACE->PWM_CH_NUM[chan].PWM_CDTY); + } +} + +/** + * DUE Board pin | PORT | Label + * ----------------+--------+------- + * 0 | PA8 | "RX0" + * 1 | PA9 | "TX0" + * 2 TIOA0 | PB25 | + * 3 TIOA7 | PC28 | + * 4 NPCS1 | PA29 | + * TIOB6 | PC26 | + * 5 TIOA6 | PC25 | + * 6 PWML7 | PC24 | + * 7 PWML6 | PC23 | + * 8 PWML5 | PC22 | + * 9 PWML4 | PC21 | + * 10 NPCS0 | PA28 | + * TIOB7 | PC29 | + * 11 TIOA8 | PD7 | + * 12 TIOB8 | PD8 | + * 13 TIOB0 | PB27 | LED AMBER "L" + * 14 TXD3 | PD4 | "TX3" + * 15 RXD3 | PD5 | "RX3" + * 16 TXD1 | PA13 | "TX2" + * 17 RXD1 | PA12 | "RX2" + * 18 TXD0 | PA11 | "TX1" + * 19 RXD0 | PA10 | "RX1" + * 20 | PB12 | "SDA" + * 21 | PB13 | "SCL" + * 22 | PB26 | + * 23 | PA14 | + * 24 | PA15 | + * 25 | PD0 | + * 26 | PD1 | + * 27 | PD2 | + * 28 | PD3 | + * 29 | PD6 | + * 30 | PD9 | + * 31 | PA7 | + * 32 | PD10 | + * 33 | PC1 | + * 34 | PC2 | + * 35 | PC3 | + * 36 | PC4 | + * 37 | PC5 | + * 38 | PC6 | + * 39 | PC7 | + * 40 | PC8 | + * 41 | PC9 | + * 42 | PA19 | + * 43 | PA20 | + * 44 | PC19 | + * 45 | PC18 | + * 46 | PC17 | + * 47 | PC16 | + * 48 | PC15 | + * 49 | PC14 | + * 50 | PC13 | + * 51 | PC12 | + * 52 NPCS2 | PB21 | + * 53 | PB14 | + * 54 | PA16 | "A0" + * 55 | PA24 | "A1" + * 56 | PA23 | "A2" + * 57 | PA22 | "A3" + * 58 TIOB2 | PA6 | "A4" + * 69 | PA4 | "A5" + * 60 TIOB1 | PA3 | "A6" + * 61 TIOA1 | PA2 | "A7" + * 62 | PB17 | "A8" + * 63 | PB18 | "A9" + * 64 | PB19 | "A10" + * 65 | PB20 | "A11" + * 66 | PB15 | "DAC0" + * 67 | PB16 | "DAC1" + * 68 | PA1 | "CANRX" + * 69 | PA0 | "CANTX" + * 70 | PA17 | "SDA1" + * 71 | PA18 | "SCL1" + * 72 | PC30 | LED AMBER "RX" + * 73 | PA21 | LED AMBER "TX" + * 74 MISO | PA25 | + * 75 MOSI | PA26 | + * 76 SCLK | PA27 | + * 77 NPCS0 | PA28 | + * 78 NPCS3 | PB23 | unconnected! + * + * USB pin | PORT + * ----------------+-------- + * ID | PB11 + * VBOF | PB10 + */ diff --git a/src/HAL/DUE/spi_pins.h b/src/HAL/DUE/spi_pins.h new file mode 100644 index 0000000..cec22c2 --- /dev/null +++ b/src/HAL/DUE/spi_pins.h @@ -0,0 +1,64 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Define SPI Pins: SCK, MISO, MOSI, SS + * + * Available chip select pins for HW SPI are 4 10 52 77 + */ +#if SDSS == 4 || SDSS == 10 || SDSS == 52 || SDSS == 77 || SDSS == 87 + #if SDSS == 4 + #define SPI_PIN 87 + #define SPI_CHAN 1 + #elif SDSS == 10 + #define SPI_PIN 77 + #define SPI_CHAN 0 + #elif SDSS == 52 + #define SPI_PIN 86 + #define SPI_CHAN 2 + #elif SDSS == 77 + #define SPI_PIN 77 + #define SPI_CHAN 0 + #else + #define SPI_PIN 87 + #define SPI_CHAN 1 + #endif + #define SD_SCK_PIN 76 + #define SD_MISO_PIN 74 + #define SD_MOSI_PIN 75 +#else + // defaults + #define DUE_SOFTWARE_SPI + #ifndef SD_SCK_PIN + #define SD_SCK_PIN 52 + #endif + #ifndef SD_MISO_PIN + #define SD_MISO_PIN 50 + #endif + #ifndef SD_MOSI_PIN + #define SD_MOSI_PIN 51 + #endif +#endif + +/* A.28, A.29, B.21, C.26, C.29 */ +#define SD_SS_PIN SDSS diff --git a/src/HAL/DUE/timers.cpp b/src/HAL/DUE/timers.cpp new file mode 100644 index 0000000..e564781 --- /dev/null +++ b/src/HAL/DUE/timers.cpp @@ -0,0 +1,139 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Timers for Arduino Due and compatible (SAM3X8E) + */ + +#ifdef ARDUINO_ARCH_SAM + +// ------------------------ +// Includes +// ------------------------ +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +// ------------------------ +// Local defines +// ------------------------ + +#define NUM_HARDWARE_TIMERS 9 + +// ------------------------ +// Private Variables +// ------------------------ + +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { + { TC0, 0, TC0_IRQn, 3}, // 0 - [servo timer5] + { TC0, 1, TC1_IRQn, 0}, // 1 + { TC0, 2, TC2_IRQn, 2}, // 2 - stepper + { TC1, 0, TC3_IRQn, 0}, // 3 - stepper for BOARD_ARCHIM1 + { TC1, 1, TC4_IRQn, 15}, // 4 - temperature + { TC1, 2, TC5_IRQn, 3}, // 5 - [servo timer3] + { TC2, 0, TC6_IRQn, 14}, // 6 - tone + { TC2, 1, TC7_IRQn, 0}, // 7 + { TC2, 2, TC8_IRQn, 0}, // 8 +}; + +// ------------------------ +// Public functions +// ------------------------ + +/* + Timer_clock1: Prescaler 2 -> 42MHz + Timer_clock2: Prescaler 8 -> 10.5MHz + Timer_clock3: Prescaler 32 -> 2.625MHz + Timer_clock4: Prescaler 128 -> 656.25kHz +*/ + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + Tc *tc = timer_config[timer_num].pTimerRegs; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + uint32_t channel = timer_config[timer_num].channel; + + // Disable interrupt, just in case it was already enabled + NVIC_DisableIRQ(irq); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + // Disable timer interrupt + tc->TC_CHANNEL[channel].TC_IDR = TC_IDR_CPCS; + + // Stop timer, just in case, to be able to reconfigure it + TC_Stop(tc, channel); + + pmc_set_writeprotect(false); + pmc_enable_periph_clk((uint32_t)irq); + NVIC_SetPriority(irq, timer_config[timer_num].priority); + + // wave mode, reset counter on match with RC, + TC_Configure(tc, channel, + TC_CMR_WAVE + | TC_CMR_WAVSEL_UP_RC + | (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) + | (HAL_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) + | (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) + | (HAL_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) + ); + + // Set compare value + TC_SetRC(tc, channel, VARIANT_MCK / (HAL_TIMER_PRESCALER) / frequency); + + // And start timer + TC_Start(tc, channel); + + // enable interrupt on RC compare + tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPCS; + + // Finally, enable IRQ + NVIC_EnableIRQ(irq); +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + NVIC_EnableIRQ(irq); +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + NVIC_DisableIRQ(irq); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); +} + +// missing from CMSIS: Check if interrupt is enabled or not +static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) { + return TEST(NVIC->ISER[uint32_t(IRQn) >> 5], uint32_t(IRQn) & 0x1F); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + return NVIC_GetEnabledIRQ(irq); +} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/timers.h b/src/HAL/DUE/timers.h new file mode 100644 index 0000000..dc35c77 --- /dev/null +++ b/src/HAL/DUE/timers.h @@ -0,0 +1,129 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Timers for Arduino Due and compatible (SAM3X8E) + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define HAL_TIMER_PRESCALER 2 +#define HAL_TIMER_RATE ((F_CPU) / (HAL_TIMER_PRESCALER)) // frequency of timers peripherals + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 2 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 4 // Timer Index for Temperature +#endif +#ifndef MF_TIMER_TONE + #define MF_TIMER_TONE 6 // index of timer to use for beeper tones +#endif + +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() void TC2_Handler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() void TC4_Handler() +#endif +#ifndef HAL_TONE_TIMER_ISR + #define HAL_TONE_TIMER_ISR() void TC6_Handler() +#endif + +// ------------------------ +// Types +// ------------------------ + +typedef struct { + Tc *pTimerRegs; + uint16_t channel; + IRQn_Type IRQ_Id; + uint8_t priority; +} tTimerConfig; + +// ------------------------ +// Public Variables +// ------------------------ + +extern const tTimerConfig timer_config[]; + +// ------------------------ +// Public functions +// ------------------------ + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + const tTimerConfig * const pConfig = &timer_config[timer_num]; + pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_RC = compare; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + const tTimerConfig * const pConfig = &timer_config[timer_num]; + return pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_RC; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + const tTimerConfig * const pConfig = &timer_config[timer_num]; + return pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_CV; +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { + const tTimerConfig * const pConfig = &timer_config[timer_num]; + // Reading the status register clears the interrupt flag + pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_SR; +} + +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/DUE/upload_extra_script.py b/src/HAL/DUE/upload_extra_script.py new file mode 100644 index 0000000..4f7a494 --- /dev/null +++ b/src/HAL/DUE/upload_extra_script.py @@ -0,0 +1,19 @@ +# +# Set upload_command +# +# Windows: bossac.exe +# Other: leave unchanged +# +import pioutil +if pioutil.is_pio_build(): + import platform + current_OS = platform.system() + + if current_OS == 'Windows': + + Import("env") + + # Use bossac.exe on Windows + env.Replace( + UPLOADCMD="bossac --info --unlock --write --verify --reset --erase -U false --boot $SOURCE" + ) diff --git a/src/HAL/DUE/usb/arduino_due_x.h b/src/HAL/DUE/usb/arduino_due_x.h new file mode 100644 index 0000000..e7b6f3d --- /dev/null +++ b/src/HAL/DUE/usb/arduino_due_x.h @@ -0,0 +1,97 @@ +/** + * \file + * + * \brief Arduino Due/X Board Definition. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +#pragma once + +/** + * Support and FAQ: visit Atmel Support + */ + +/** + * \page arduino_due_x_opfreq "Arduino Due/X - Operating frequencies" + * This page lists several definition related to the board operating frequency + * + * \section Definitions + * - \ref BOARD_FREQ_* + * - \ref BOARD_MCK + */ + +/*! Board oscillator settings */ +#define BOARD_FREQ_SLCK_XTAL (32768U) +#define BOARD_FREQ_SLCK_BYPASS (32768U) +#define BOARD_FREQ_MAINCK_XTAL (12000000U) +#define BOARD_FREQ_MAINCK_BYPASS (12000000U) + +/*! Master clock frequency */ +#define BOARD_MCK CHIP_FREQ_CPU_MAX +#define BOARD_NO_32K_XTAL + +/** board main clock xtal startup time */ +#define BOARD_OSC_STARTUP_US 15625 + +/* ------------------------------------------------------------------------ */ + +/** + * \page arduino_due_x_board_info "Arduino Due/X - Board information" + * This page lists several definition related to the board description. + * + */ + +/* ------------------------------------------------------------------------ */ +/* USB */ +/* ------------------------------------------------------------------------ */ +/*! USB OTG VBus On/Off: Bus Power Control Port. */ +#define PIN_UOTGHS_VBOF { PIO_PB10, PIOB, ID_PIOB, PIO_PERIPH_A, PIO_PULLUP } +/*! USB OTG Identification: Mini Connector Identification Port. */ +#define PIN_UOTGHS_ID { PIO_PB11, PIOB, ID_PIOB, PIO_PERIPH_A, PIO_PULLUP } + +/*! Multiplexed pin used for USB_ID: */ +#define USB_ID PIO_PB11_IDX +#define USB_ID_GPIO (PIO_PB11_IDX) +#define USB_ID_FLAGS (PIO_PERIPH_A | PIO_DEFAULT) +/*! Multiplexed pin used for USB_VBOF: */ +#define USB_VBOF PIO_PB10_IDX +#define USB_VBOF_GPIO (PIO_PB10_IDX) +#define USB_VBOF_FLAGS (PIO_PERIPH_A | PIO_DEFAULT) +/*! Active level of the USB_VBOF output pin. */ +#define USB_VBOF_ACTIVE_STATE LOW +/* ------------------------------------------------------------------------ */ diff --git a/src/HAL/DUE/usb/compiler.h b/src/HAL/DUE/usb/compiler.h new file mode 100644 index 0000000..6331979 --- /dev/null +++ b/src/HAL/DUE/usb/compiler.h @@ -0,0 +1,1150 @@ +/** + * \file + * + * \brief Commonly used includes, types and macros. + * + * Copyright (c) 2010-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef UTILS_COMPILER_H +#define UTILS_COMPILER_H + +#include +#include +#include "arduino_due_x.h" +#include "conf_clock.h" +#ifdef SAM3XA_SERIES +#define SAM3XA 1 +#endif +#define UDD_NO_SLEEP_MGR 1 +#define pmc_is_wakeup_clocks_restored() true + +#undef udd_get_endpoint_size_max +#define UDD_USB_INT_FUN USBD_ISR + +/** + * \defgroup group_sam_utils Compiler abstraction layer and code utilities + * + * Compiler abstraction layer and code utilities for AT91SAM. + * This module provides various abstraction layers and utilities to make code compatible between different compilers. + * + * \{ + */ +#include + +#if (defined __ICCARM__) +# include +#endif + +#include +#include "preprocessor.h" + +//_____ D E C L A R A T I O N S ____________________________________________ + +#ifndef __ASSEMBLY__ // Not defined for assembling. + +#include +#include +#include +#include + +#ifdef __ICCARM__ +/*! \name Compiler Keywords + * + * Port of some keywords from GCC to IAR Embedded Workbench. + */ +//! @{ +#define __asm__ asm +#define __inline__ inline +#define __volatile__ +//! @} + +#endif + +#define FUNC_PTR void * +/** + * \def UNUSED + * \brief Marking \a v as a unused parameter or value. + */ +#ifndef UNUSED + #define UNUSED(x) ((void)(x)) +#endif + +/** + * \def unused + * \brief Marking \a v as a unused parameter or value. + */ +#define unused(v) do { (void)(v); }while(0) + +/** + * \def barrier + * \brief Memory barrier + */ +#define barrier() __DMB() + +/** + * \brief Emit the compiler pragma \a arg. + * + * \param arg The pragma directive as it would appear after \e \#pragma + * (i.e. not stringified). + */ +#define COMPILER_PRAGMA(arg) _Pragma(#arg) + +/** + * \def COMPILER_PACK_SET(alignment) + * \brief Set maximum alignment for subsequent struct and union + * definitions to \a alignment. + */ +#define COMPILER_PACK_SET(alignment) COMPILER_PRAGMA(pack(alignment)) + +/** + * \def COMPILER_PACK_RESET() + * \brief Set default alignment for subsequent struct and union + * definitions. + */ +#define COMPILER_PACK_RESET() COMPILER_PRAGMA(pack()) + + +/** + * \brief Set aligned boundary. + */ +#if (defined __GNUC__) || (defined __CC_ARM) +# define COMPILER_ALIGNED(a) __attribute__((__aligned__(a))) +#elif (defined __ICCARM__) +# define COMPILER_ALIGNED(a) COMPILER_PRAGMA(data_alignment = a) +#endif + +/** + * \brief Set word-aligned boundary. + */ +#if (defined __GNUC__) || defined(__CC_ARM) +#define COMPILER_WORD_ALIGNED __attribute__((__aligned__(4))) +#elif (defined __ICCARM__) +#define COMPILER_WORD_ALIGNED COMPILER_PRAGMA(data_alignment = 4) +#endif + +/** + * \def __always_inline + * \brief The function should always be inlined. + * + * This annotation instructs the compiler to ignore its inlining + * heuristics and inline the function no matter how big it thinks it + * becomes. + */ +#ifdef __CC_ARM +# define __always_inline __forceinline +#elif (defined __GNUC__) +#ifdef __always_inline +# undef __always_inline +#endif +# define __always_inline inline __attribute__((__always_inline__)) +#elif (defined __ICCARM__) +# define __always_inline _Pragma("inline=forced") +#endif + +/** + * \def __no_inline + * \brief The function should not be inlined. + * + * This annotation instructs the compiler to ignore its inlining + * heuristics and not inline the function. + */ +#ifdef __CC_ARM +# define __no_inline __attribute__((noinline)) +#elif (defined __GNUC__) +# define __no_inline __attribute__((__noinline__)) +#elif (defined __ICCARM__) +# define __no_inline _Pragma("inline=never") +#endif + +/*! \brief This macro is used to test fatal errors. + * + * The macro tests if the expression is false. If it is, a fatal error is + * detected and the application hangs up. If TEST_SUITE_DEFINE_ASSERT_MACRO + * is defined, a unit test version of the macro is used, to allow execution + * of further tests after a false expression. + * + * \param expr Expression to evaluate and supposed to be nonzero. + */ +#ifdef _ASSERT_ENABLE_ +# if defined(TEST_SUITE_DEFINE_ASSERT_MACRO) + // Assert() is defined in unit_test/suite.h +# include "unit_test/suite.h" +# else +#undef TEST_SUITE_DEFINE_ASSERT_MACRO +# define Assert(expr) \ + {\ + if (!(expr)) while (true);\ + } +# endif +#else +# define Assert(expr) ((void) 0) +#endif + +/* Define WEAK attribute */ +#if defined ( __CC_ARM ) /* Keil µVision 4 */ +# define WEAK __attribute__ ((weak)) +#elif defined ( __ICCARM__ ) /* IAR Ewarm 5.41+ */ +# define WEAK __weak +#elif defined ( __GNUC__ ) /* GCC CS3 2009q3-68 */ +# define WEAK __attribute__ ((weak)) +#endif + +/* Define NO_INIT attribute */ +#if 0 //ndef NO_INIT +#ifdef __CC_ARM +# define NO_INIT __attribute__((zero_init)) +#elif defined ( __ICCARM__ ) +# define NO_INIT __no_init +#elif defined ( __GNUC__ ) +# define NO_INIT __attribute__((section(".no_init"))) +#endif +#endif + +/* Define RAMFUNC attribute */ +#if defined ( __CC_ARM ) /* Keil µVision 4 */ +# define RAMFUNC __attribute__ ((section(".ramfunc"))) +#elif defined ( __ICCARM__ ) /* IAR Ewarm 5.41+ */ +# define RAMFUNC __ramfunc +#elif defined ( __GNUC__ ) /* GCC CS3 2009q3-68 */ +# define RAMFUNC __attribute__ ((section(".ramfunc"))) +#endif + +/* Define OPTIMIZE_HIGH attribute */ +#if defined ( __CC_ARM ) /* Keil µVision 4 */ +# define OPTIMIZE_HIGH _Pragma("O3") +#elif defined ( __ICCARM__ ) /* IAR Ewarm 5.41+ */ +# define OPTIMIZE_HIGH _Pragma("optimize=high") +#elif defined ( __GNUC__ ) /* GCC CS3 2009q3-68 */ +# define OPTIMIZE_HIGH __attribute__((optimize("s"))) +#endif + +/*! \name Usual Types + */ +//! @{ +typedef unsigned char Bool; //!< Boolean. +#ifndef __cplusplus +#ifndef __bool_true_false_are_defined +typedef unsigned char bool; //!< Boolean. +#endif +#endif +typedef int8_t S8 ; //!< 8-bit signed integer. +typedef uint8_t U8 ; //!< 8-bit unsigned integer. +typedef int16_t S16; //!< 16-bit signed integer. +typedef uint16_t U16; //!< 16-bit unsigned integer. +typedef uint16_t le16_t; +typedef uint16_t be16_t; +typedef int32_t S32; //!< 32-bit signed integer. +typedef uint32_t U32; //!< 32-bit unsigned integer. +typedef uint32_t le32_t; +typedef uint32_t be32_t; +typedef int64_t S64; //!< 64-bit signed integer. +typedef uint64_t U64; //!< 64-bit unsigned integer. +typedef float F32; //!< 32-bit floating-point number. +typedef double F64; //!< 64-bit floating-point number. +typedef uint32_t iram_size_t; +//! @} + + +/*! \name Status Types + */ +//! @{ +typedef bool Status_bool_t; //!< Boolean status. +typedef U8 Status_t; //!< 8-bit-coded status. +//! @} + + +/*! \name Aliasing Aggregate Types + */ +//! @{ + +//! 16-bit union. +typedef union +{ + S16 s16 ; + U16 u16 ; + S8 s8 [2]; + U8 u8 [2]; +} Union16; + +//! 32-bit union. +typedef union +{ + S32 s32 ; + U32 u32 ; + S16 s16[2]; + U16 u16[2]; + S8 s8 [4]; + U8 u8 [4]; +} Union32; + +//! 64-bit union. +typedef union +{ + S64 s64 ; + U64 u64 ; + S32 s32[2]; + U32 u32[2]; + S16 s16[4]; + U16 u16[4]; + S8 s8 [8]; + U8 u8 [8]; +} Union64; + +//! Union of pointers to 64-, 32-, 16- and 8-bit unsigned integers. +typedef union +{ + S64 *s64ptr; + U64 *u64ptr; + S32 *s32ptr; + U32 *u32ptr; + S16 *s16ptr; + U16 *u16ptr; + S8 *s8ptr ; + U8 *u8ptr ; +} UnionPtr; + +//! Union of pointers to volatile 64-, 32-, 16- and 8-bit unsigned integers. +typedef union +{ + volatile S64 *s64ptr; + volatile U64 *u64ptr; + volatile S32 *s32ptr; + volatile U32 *u32ptr; + volatile S16 *s16ptr; + volatile U16 *u16ptr; + volatile S8 *s8ptr ; + volatile U8 *u8ptr ; +} UnionVPtr; + +//! Union of pointers to constant 64-, 32-, 16- and 8-bit unsigned integers. +typedef union +{ + const S64 *s64ptr; + const U64 *u64ptr; + const S32 *s32ptr; + const U32 *u32ptr; + const S16 *s16ptr; + const U16 *u16ptr; + const S8 *s8ptr ; + const U8 *u8ptr ; +} UnionCPtr; + +//! Union of pointers to constant volatile 64-, 32-, 16- and 8-bit unsigned integers. +typedef union +{ + const volatile S64 *s64ptr; + const volatile U64 *u64ptr; + const volatile S32 *s32ptr; + const volatile U32 *u32ptr; + const volatile S16 *s16ptr; + const volatile U16 *u16ptr; + const volatile S8 *s8ptr ; + const volatile U8 *u8ptr ; +} UnionCVPtr; + +//! Structure of pointers to 64-, 32-, 16- and 8-bit unsigned integers. +typedef struct +{ + S64 *s64ptr; + U64 *u64ptr; + S32 *s32ptr; + U32 *u32ptr; + S16 *s16ptr; + U16 *u16ptr; + S8 *s8ptr ; + U8 *u8ptr ; +} StructPtr; + +//! Structure of pointers to volatile 64-, 32-, 16- and 8-bit unsigned integers. +typedef struct +{ + volatile S64 *s64ptr; + volatile U64 *u64ptr; + volatile S32 *s32ptr; + volatile U32 *u32ptr; + volatile S16 *s16ptr; + volatile U16 *u16ptr; + volatile S8 *s8ptr ; + volatile U8 *u8ptr ; +} StructVPtr; + +//! Structure of pointers to constant 64-, 32-, 16- and 8-bit unsigned integers. +typedef struct +{ + const S64 *s64ptr; + const U64 *u64ptr; + const S32 *s32ptr; + const U32 *u32ptr; + const S16 *s16ptr; + const U16 *u16ptr; + const S8 *s8ptr ; + const U8 *u8ptr ; +} StructCPtr; + +//! Structure of pointers to constant volatile 64-, 32-, 16- and 8-bit unsigned integers. +typedef struct +{ + const volatile S64 *s64ptr; + const volatile U64 *u64ptr; + const volatile S32 *s32ptr; + const volatile U32 *u32ptr; + const volatile S16 *s16ptr; + const volatile U16 *u16ptr; + const volatile S8 *s8ptr ; + const volatile U8 *u8ptr ; +} StructCVPtr; + +//! @} + +#endif // #ifndef __ASSEMBLY__ + +/*! \name Usual Constants + */ +//! @{ +#define DISABLE 0 +#define ENABLE 1 +#ifndef __cplusplus +#ifndef __bool_true_false_are_defined +#define false (1==0) +#define true (1==1) +#endif +#endif +#ifndef PASS +#define PASS 0 +#endif +#ifndef FAIL +#define FAIL 1 +#endif +#ifndef LOW +#define LOW 0x0 +#endif +#ifndef HIGH +#define HIGH 0x1 +#endif +//! @} + + +#ifndef __ASSEMBLY__ // not for assembling. + +//! \name Optimization Control +//@{ + +/** + * \def likely(exp) + * \brief The expression \a exp is likely to be true + */ +#ifndef likely +# define likely(exp) (exp) +#endif + +/** + * \def unlikely(exp) + * \brief The expression \a exp is unlikely to be true + */ +#ifndef unlikely +# define unlikely(exp) (exp) +#endif + +/** + * \def is_constant(exp) + * \brief Determine if an expression evaluates to a constant value. + * + * \param exp Any expression + * + * \return true if \a exp is constant, false otherwise. + */ +#if (defined __GNUC__) || (defined __CC_ARM) +# define is_constant(exp) __builtin_constant_p(exp) +#else +# define is_constant(exp) (0) +#endif + +//! @} + +/*! \name Bit-Field Handling + */ +//! @{ + +/*! \brief Reads the bits of a value specified by a given bit-mask. + * + * \param value Value to read bits from. + * \param mask Bit-mask indicating bits to read. + * + * \return Read bits. + */ +#define Rd_bits( value, mask) ((value) & (mask)) + +/*! \brief Writes the bits of a C lvalue specified by a given bit-mask. + * + * \param lvalue C lvalue to write bits to. + * \param mask Bit-mask indicating bits to write. + * \param bits Bits to write. + * + * \return Resulting value with written bits. + */ +#define Wr_bits(lvalue, mask, bits) ((lvalue) = ((lvalue) & ~(mask)) |\ + ((bits ) & (mask))) + +/*! \brief Tests the bits of a value specified by a given bit-mask. + * + * \param value Value of which to test bits. + * \param mask Bit-mask indicating bits to test. + * + * \return \c 1 if at least one of the tested bits is set, else \c 0. + */ +#define Tst_bits( value, mask) (Rd_bits(value, mask) != 0) + +/*! \brief Clears the bits of a C lvalue specified by a given bit-mask. + * + * \param lvalue C lvalue of which to clear bits. + * \param mask Bit-mask indicating bits to clear. + * + * \return Resulting value with cleared bits. + */ +#define Clr_bits(lvalue, mask) ((lvalue) &= ~(mask)) + +/*! \brief Sets the bits of a C lvalue specified by a given bit-mask. + * + * \param lvalue C lvalue of which to set bits. + * \param mask Bit-mask indicating bits to set. + * + * \return Resulting value with set bits. + */ +#define Set_bits(lvalue, mask) ((lvalue) |= (mask)) + +/*! \brief Toggles the bits of a C lvalue specified by a given bit-mask. + * + * \param lvalue C lvalue of which to toggle bits. + * \param mask Bit-mask indicating bits to toggle. + * + * \return Resulting value with toggled bits. + */ +#define Tgl_bits(lvalue, mask) ((lvalue) ^= (mask)) + +/*! \brief Reads the bit-field of a value specified by a given bit-mask. + * + * \param value Value to read a bit-field from. + * \param mask Bit-mask indicating the bit-field to read. + * + * \return Read bit-field. + */ +#define Rd_bitfield( value, mask) (Rd_bits( value, mask) >> ctz(mask)) + +/*! \brief Writes the bit-field of a C lvalue specified by a given bit-mask. + * + * \param lvalue C lvalue to write a bit-field to. + * \param mask Bit-mask indicating the bit-field to write. + * \param bitfield Bit-field to write. + * + * \return Resulting value with written bit-field. + */ +#define Wr_bitfield(lvalue, mask, bitfield) (Wr_bits(lvalue, mask, (U32)(bitfield) << ctz(mask))) + +//! @} + + +/*! \name Zero-Bit Counting + * + * Under GCC, __builtin_clz and __builtin_ctz behave like macros when + * applied to constant expressions (values known at compile time), so they are + * more optimized than the use of the corresponding assembly instructions and + * they can be used as constant expressions e.g. to initialize objects having + * static storage duration, and like the corresponding assembly instructions + * when applied to non-constant expressions (values unknown at compile time), so + * they are more optimized than an assembly periphrasis. Hence, clz and ctz + * ensure a possible and optimized behavior for both constant and non-constant + * expressions. + */ +//! @{ + +/*! \brief Counts the leading zero bits of the given value considered as a 32-bit integer. + * + * \param u Value of which to count the leading zero bits. + * + * \return The count of leading zero bits in \a u. + */ +#ifndef clz +#if (defined __GNUC__) || (defined __CC_ARM) +# define clz(u) ((u) ? __builtin_clz(u) : 32) +#elif (defined __ICCARM__) +# define clz(u) ((u) ? __CLZ(u) : 32) +#else +# define clz(u) (((u) == 0) ? 32 : \ + ((u) & (1UL << 31)) ? 0 : \ + ((u) & (1UL << 30)) ? 1 : \ + ((u) & (1UL << 29)) ? 2 : \ + ((u) & (1UL << 28)) ? 3 : \ + ((u) & (1UL << 27)) ? 4 : \ + ((u) & (1UL << 26)) ? 5 : \ + ((u) & (1UL << 25)) ? 6 : \ + ((u) & (1UL << 24)) ? 7 : \ + ((u) & (1UL << 23)) ? 8 : \ + ((u) & (1UL << 22)) ? 9 : \ + ((u) & (1UL << 21)) ? 10 : \ + ((u) & (1UL << 20)) ? 11 : \ + ((u) & (1UL << 19)) ? 12 : \ + ((u) & (1UL << 18)) ? 13 : \ + ((u) & (1UL << 17)) ? 14 : \ + ((u) & (1UL << 16)) ? 15 : \ + ((u) & (1UL << 15)) ? 16 : \ + ((u) & (1UL << 14)) ? 17 : \ + ((u) & (1UL << 13)) ? 18 : \ + ((u) & (1UL << 12)) ? 19 : \ + ((u) & (1UL << 11)) ? 20 : \ + ((u) & (1UL << 10)) ? 21 : \ + ((u) & (1UL << 9)) ? 22 : \ + ((u) & (1UL << 8)) ? 23 : \ + ((u) & (1UL << 7)) ? 24 : \ + ((u) & (1UL << 6)) ? 25 : \ + ((u) & (1UL << 5)) ? 26 : \ + ((u) & (1UL << 4)) ? 27 : \ + ((u) & (1UL << 3)) ? 28 : \ + ((u) & (1UL << 2)) ? 29 : \ + ((u) & (1UL << 1)) ? 30 : \ + 31) +#endif +#endif + +/*! \brief Counts the trailing zero bits of the given value considered as a 32-bit integer. + * + * \param u Value of which to count the trailing zero bits. + * + * \return The count of trailing zero bits in \a u. + */ +#ifndef ctz +#if (defined __GNUC__) || (defined __CC_ARM) +# define ctz(u) ((u) ? __builtin_ctz(u) : 32) +#else +# define ctz(u) ((u) & (1UL << 0) ? 0 : \ + (u) & (1UL << 1) ? 1 : \ + (u) & (1UL << 2) ? 2 : \ + (u) & (1UL << 3) ? 3 : \ + (u) & (1UL << 4) ? 4 : \ + (u) & (1UL << 5) ? 5 : \ + (u) & (1UL << 6) ? 6 : \ + (u) & (1UL << 7) ? 7 : \ + (u) & (1UL << 8) ? 8 : \ + (u) & (1UL << 9) ? 9 : \ + (u) & (1UL << 10) ? 10 : \ + (u) & (1UL << 11) ? 11 : \ + (u) & (1UL << 12) ? 12 : \ + (u) & (1UL << 13) ? 13 : \ + (u) & (1UL << 14) ? 14 : \ + (u) & (1UL << 15) ? 15 : \ + (u) & (1UL << 16) ? 16 : \ + (u) & (1UL << 17) ? 17 : \ + (u) & (1UL << 18) ? 18 : \ + (u) & (1UL << 19) ? 19 : \ + (u) & (1UL << 20) ? 20 : \ + (u) & (1UL << 21) ? 21 : \ + (u) & (1UL << 22) ? 22 : \ + (u) & (1UL << 23) ? 23 : \ + (u) & (1UL << 24) ? 24 : \ + (u) & (1UL << 25) ? 25 : \ + (u) & (1UL << 26) ? 26 : \ + (u) & (1UL << 27) ? 27 : \ + (u) & (1UL << 28) ? 28 : \ + (u) & (1UL << 29) ? 29 : \ + (u) & (1UL << 30) ? 30 : \ + (u) & (1UL << 31) ? 31 : \ + 32) +#endif +#endif + +//! @} + + +/*! \name Bit Reversing + */ +//! @{ + +/*! \brief Reverses the bits of \a u8. + * + * \param u8 U8 of which to reverse the bits. + * + * \return Value resulting from \a u8 with reversed bits. + */ +#define bit_reverse8(u8) ((U8)(bit_reverse32((U8)(u8)) >> 24)) + +/*! \brief Reverses the bits of \a u16. + * + * \param u16 U16 of which to reverse the bits. + * + * \return Value resulting from \a u16 with reversed bits. + */ +#define bit_reverse16(u16) ((U16)(bit_reverse32((U16)(u16)) >> 16)) + +/*! \brief Reverses the bits of \a u32. + * + * \param u32 U32 of which to reverse the bits. + * + * \return Value resulting from \a u32 with reversed bits. + */ +#define bit_reverse32(u32) __RBIT(u32) + +/*! \brief Reverses the bits of \a u64. + * + * \param u64 U64 of which to reverse the bits. + * + * \return Value resulting from \a u64 with reversed bits. + */ +#define bit_reverse64(u64) ((U64)(((U64)bit_reverse32((U64)(u64) >> 32)) |\ + ((U64)bit_reverse32((U64)(u64)) << 32))) + +//! @} + + +/*! \name Alignment + */ +//! @{ + +/*! \brief Tests alignment of the number \a val with the \a n boundary. + * + * \param val Input value. + * \param n Boundary. + * + * \return \c 1 if the number \a val is aligned with the \a n boundary, else \c 0. + */ +#define Test_align(val, n ) (!Tst_bits( val, (n) - 1 ) ) + +/*! \brief Gets alignment of the number \a val with respect to the \a n boundary. + * + * \param val Input value. + * \param n Boundary. + * + * \return Alignment of the number \a val with respect to the \a n boundary. + */ +#define Get_align( val, n ) ( Rd_bits( val, (n) - 1 ) ) + +/*! \brief Sets alignment of the lvalue number \a lval to \a alg with respect to the \a n boundary. + * + * \param lval Input/output lvalue. + * \param n Boundary. + * \param alg Alignment. + * + * \return New value of \a lval resulting from its alignment set to \a alg with respect to the \a n boundary. + */ +#define Set_align(lval, n, alg) ( Wr_bits(lval, (n) - 1, alg) ) + +/*! \brief Aligns the number \a val with the upper \a n boundary. + * + * \param val Input value. + * \param n Boundary. + * + * \return Value resulting from the number \a val aligned with the upper \a n boundary. + */ +#define Align_up( val, n ) (((val) + ((n) - 1)) & ~((n) - 1)) + +/*! \brief Aligns the number \a val with the lower \a n boundary. + * + * \param val Input value. + * \param n Boundary. + * + * \return Value resulting from the number \a val aligned with the lower \a n boundary. + */ +#define Align_down(val, n ) ( (val) & ~((n) - 1)) + +//! @} + +/*! \brief Calls the routine at address \a addr. + * + * It generates a long call opcode. + * + * For example, `Long_call(0x80000000)' generates a software reset on a UC3 if + * it is invoked from the CPU supervisor mode. + * + * \param addr Address of the routine to call. + * + * \note It may be used as a long jump opcode in some special cases. + */ +#define Long_call(addr) ((*(void (*)(void))(addr))()) + + +/*! \name MCU Endianism Handling + * ARM is MCU little endianism. + */ +//! @{ +#define MSB(u16) (((U8 *)&(u16))[1]) //!< Most significant byte of \a u16. +#define LSB(u16) (((U8 *)&(u16))[0]) //!< Least significant byte of \a u16. + +#define MSH(u32) (((U16 *)&(u32))[1]) //!< Most significant half-word of \a u32. +#define LSH(u32) (((U16 *)&(u32))[0]) //!< Least significant half-word of \a u32. +#define MSB0W(u32) (((U8 *)&(u32))[3]) //!< Most significant byte of 1st rank of \a u32. +#define MSB1W(u32) (((U8 *)&(u32))[2]) //!< Most significant byte of 2nd rank of \a u32. +#define MSB2W(u32) (((U8 *)&(u32))[1]) //!< Most significant byte of 3rd rank of \a u32. +#define MSB3W(u32) (((U8 *)&(u32))[0]) //!< Most significant byte of 4th rank of \a u32. +#define LSB3W(u32) MSB0W(u32) //!< Least significant byte of 4th rank of \a u32. +#define LSB2W(u32) MSB1W(u32) //!< Least significant byte of 3rd rank of \a u32. +#define LSB1W(u32) MSB2W(u32) //!< Least significant byte of 2nd rank of \a u32. +#define LSB0W(u32) MSB3W(u32) //!< Least significant byte of 1st rank of \a u32. + +#define MSW(u64) (((U32 *)&(u64))[1]) //!< Most significant word of \a u64. +#define LSW(u64) (((U32 *)&(u64))[0]) //!< Least significant word of \a u64. +#define MSH0(u64) (((U16 *)&(u64))[3]) //!< Most significant half-word of 1st rank of \a u64. +#define MSH1(u64) (((U16 *)&(u64))[2]) //!< Most significant half-word of 2nd rank of \a u64. +#define MSH2(u64) (((U16 *)&(u64))[1]) //!< Most significant half-word of 3rd rank of \a u64. +#define MSH3(u64) (((U16 *)&(u64))[0]) //!< Most significant half-word of 4th rank of \a u64. +#define LSH3(u64) MSH0(u64) //!< Least significant half-word of 4th rank of \a u64. +#define LSH2(u64) MSH1(u64) //!< Least significant half-word of 3rd rank of \a u64. +#define LSH1(u64) MSH2(u64) //!< Least significant half-word of 2nd rank of \a u64. +#define LSH0(u64) MSH3(u64) //!< Least significant half-word of 1st rank of \a u64. +#define MSB0D(u64) (((U8 *)&(u64))[7]) //!< Most significant byte of 1st rank of \a u64. +#define MSB1D(u64) (((U8 *)&(u64))[6]) //!< Most significant byte of 2nd rank of \a u64. +#define MSB2D(u64) (((U8 *)&(u64))[5]) //!< Most significant byte of 3rd rank of \a u64. +#define MSB3D(u64) (((U8 *)&(u64))[4]) //!< Most significant byte of 4th rank of \a u64. +#define MSB4D(u64) (((U8 *)&(u64))[3]) //!< Most significant byte of 5th rank of \a u64. +#define MSB5D(u64) (((U8 *)&(u64))[2]) //!< Most significant byte of 6th rank of \a u64. +#define MSB6D(u64) (((U8 *)&(u64))[1]) //!< Most significant byte of 7th rank of \a u64. +#define MSB7D(u64) (((U8 *)&(u64))[0]) //!< Most significant byte of 8th rank of \a u64. +#define LSB7D(u64) MSB0D(u64) //!< Least significant byte of 8th rank of \a u64. +#define LSB6D(u64) MSB1D(u64) //!< Least significant byte of 7th rank of \a u64. +#define LSB5D(u64) MSB2D(u64) //!< Least significant byte of 6th rank of \a u64. +#define LSB4D(u64) MSB3D(u64) //!< Least significant byte of 5th rank of \a u64. +#define LSB3D(u64) MSB4D(u64) //!< Least significant byte of 4th rank of \a u64. +#define LSB2D(u64) MSB5D(u64) //!< Least significant byte of 3rd rank of \a u64. +#define LSB1D(u64) MSB6D(u64) //!< Least significant byte of 2nd rank of \a u64. +#define LSB0D(u64) MSB7D(u64) //!< Least significant byte of 1st rank of \a u64. + +#define BE16(x) swap16(x) +#define LE16(x) (x) + +#define le16_to_cpu(x) (x) +#define cpu_to_le16(x) (x) +#define LE16_TO_CPU(x) (x) +#define CPU_TO_LE16(x) (x) + +#define be16_to_cpu(x) swap16(x) +#define cpu_to_be16(x) swap16(x) +#define BE16_TO_CPU(x) swap16(x) +#define CPU_TO_BE16(x) swap16(x) + +#define le32_to_cpu(x) (x) +#define cpu_to_le32(x) (x) +#define LE32_TO_CPU(x) (x) +#define CPU_TO_LE32(x) (x) + +#define be32_to_cpu(x) swap32(x) +#define cpu_to_be32(x) swap32(x) +#define BE32_TO_CPU(x) swap32(x) +#define CPU_TO_BE32(x) swap32(x) +//! @} + + +/*! \name Endianism Conversion + * + * The same considerations as for clz and ctz apply here but GCC's + * __builtin_bswap_32 and __builtin_bswap_64 do not behave like macros when + * applied to constant expressions, so two sets of macros are defined here: + * - Swap16, Swap32 and Swap64 to apply to constant expressions (values known + * at compile time); + * - swap16, swap32 and swap64 to apply to non-constant expressions (values + * unknown at compile time). + */ +//! @{ + +/*! \brief Toggles the endianism of \a u16 (by swapping its bytes). + * + * \param u16 U16 of which to toggle the endianism. + * + * \return Value resulting from \a u16 with toggled endianism. + * + * \note More optimized if only used with values known at compile time. + */ +#define Swap16(u16) ((U16)(((U16)(u16) >> 8) |\ + ((U16)(u16) << 8))) + +/*! \brief Toggles the endianism of \a u32 (by swapping its bytes). + * + * \param u32 U32 of which to toggle the endianism. + * + * \return Value resulting from \a u32 with toggled endianism. + * + * \note More optimized if only used with values known at compile time. + */ +#define Swap32(u32) ((U32)(((U32)Swap16((U32)(u32) >> 16)) |\ + ((U32)Swap16((U32)(u32)) << 16))) + +/*! \brief Toggles the endianism of \a u64 (by swapping its bytes). + * + * \param u64 U64 of which to toggle the endianism. + * + * \return Value resulting from \a u64 with toggled endianism. + * + * \note More optimized if only used with values known at compile time. + */ +#define Swap64(u64) ((U64)(((U64)Swap32((U64)(u64) >> 32)) |\ + ((U64)Swap32((U64)(u64)) << 32))) + +/*! \brief Toggles the endianism of \a u16 (by swapping its bytes). + * + * \param u16 U16 of which to toggle the endianism. + * + * \return Value resulting from \a u16 with toggled endianism. + * + * \note More optimized if only used with values unknown at compile time. + */ +#define swap16(u16) Swap16(u16) + +/*! \brief Toggles the endianism of \a u32 (by swapping its bytes). + * + * \param u32 U32 of which to toggle the endianism. + * + * \return Value resulting from \a u32 with toggled endianism. + * + * \note More optimized if only used with values unknown at compile time. + */ +#if (defined __GNUC__) +# define swap32(u32) ((U32)__builtin_bswap32((U32)(u32))) +#else +# define swap32(u32) Swap32(u32) +#endif + +/*! \brief Toggles the endianism of \a u64 (by swapping its bytes). + * + * \param u64 U64 of which to toggle the endianism. + * + * \return Value resulting from \a u64 with toggled endianism. + * + * \note More optimized if only used with values unknown at compile time. + */ +#if (defined __GNUC__) +# define swap64(u64) ((U64)__builtin_bswap64((U64)(u64))) +#else +# define swap64(u64) ((U64)(((U64)swap32((U64)(u64) >> 32)) |\ + ((U64)swap32((U64)(u64)) << 32))) +#endif + +//! @} + + +/*! \name Target Abstraction + */ +//! @{ + +#define _GLOBEXT_ extern //!< extern storage-class specifier. +#define _CONST_TYPE_ const //!< const type qualifier. +#define _MEM_TYPE_SLOW_ //!< Slow memory type. +#define _MEM_TYPE_MEDFAST_ //!< Fairly fast memory type. +#define _MEM_TYPE_FAST_ //!< Fast memory type. + +typedef U8 Byte; //!< 8-bit unsigned integer. + +#define memcmp_ram2ram memcmp //!< Target-specific memcmp of RAM to RAM. +#define memcmp_code2ram memcmp //!< Target-specific memcmp of RAM to NVRAM. +#define memcpy_ram2ram memcpy //!< Target-specific memcpy from RAM to RAM. +#define memcpy_code2ram memcpy //!< Target-specific memcpy from NVRAM to RAM. + +#define LSB0(u32) LSB0W(u32) //!< Least significant byte of 1st rank of \a u32. +#define LSB1(u32) LSB1W(u32) //!< Least significant byte of 2nd rank of \a u32. +#define LSB2(u32) LSB2W(u32) //!< Least significant byte of 3rd rank of \a u32. +#define LSB3(u32) LSB3W(u32) //!< Least significant byte of 4th rank of \a u32. +#define MSB3(u32) MSB3W(u32) //!< Most significant byte of 4th rank of \a u32. +#define MSB2(u32) MSB2W(u32) //!< Most significant byte of 3rd rank of \a u32. +#define MSB1(u32) MSB1W(u32) //!< Most significant byte of 2nd rank of \a u32. +#define MSB0(u32) MSB0W(u32) //!< Most significant byte of 1st rank of \a u32. + +//! @} + +/** + * \brief Calculate \f$ \left\lceil \frac{a}{b} \right\rceil \f$ using + * integer arithmetic. + * + * \param a An integer + * \param b Another integer + * + * \return (\a a / \a b) rounded up to the nearest integer. + */ +#define div_ceil(a, b) (((a) + (b) - 1) / (b)) + +#endif // #ifndef __ASSEMBLY__ + + +#ifdef __ICCARM__ +#define SHORTENUM __packed +#elif defined(__GNUC__) +#define SHORTENUM __attribute__((packed)) +#endif + +/* No operation */ +#ifdef __ICCARM__ +#define nop() __no_operation() +#elif defined(__GNUC__) +#define nop() (__NOP()) +#endif + +#define FLASH_DECLARE(x) const x +#define FLASH_EXTERN(x) extern const x +#define PGM_READ_BYTE(x) *(x) +#define PGM_READ_WORD(x) *(x) +#define PGM_READ_DWORD(x) *(x) +#define MEMCPY_ENDIAN memcpy +#define PGM_READ_BLOCK(dst, src, len) memcpy((dst), (src), (len)) + +/*Defines the Flash Storage for the request and response of MAC*/ +#define CMD_ID_OCTET (0) + +/* Converting of values from CPU endian to little endian. */ +#define CPU_ENDIAN_TO_LE16(x) (x) +#define CPU_ENDIAN_TO_LE32(x) (x) +#define CPU_ENDIAN_TO_LE64(x) (x) + +/* Converting of values from little endian to CPU endian. */ +#define LE16_TO_CPU_ENDIAN(x) (x) +#define LE32_TO_CPU_ENDIAN(x) (x) +#define LE64_TO_CPU_ENDIAN(x) (x) + +/* Converting of constants from little endian to CPU endian. */ +#define CLE16_TO_CPU_ENDIAN(x) (x) +#define CLE32_TO_CPU_ENDIAN(x) (x) +#define CLE64_TO_CPU_ENDIAN(x) (x) + +/* Converting of constants from CPU endian to little endian. */ +#define CCPU_ENDIAN_TO_LE16(x) (x) +#define CCPU_ENDIAN_TO_LE32(x) (x) +#define CCPU_ENDIAN_TO_LE64(x) (x) + +#define ADDR_COPY_DST_SRC_16(dst, src) ((dst) = (src)) +#define ADDR_COPY_DST_SRC_64(dst, src) ((dst) = (src)) + +/** + * @brief Converts a 64-Bit value into a 8 Byte array + * + * @param[in] value 64-Bit value + * @param[out] data Pointer to the 8 Byte array to be updated with 64-Bit value + * @ingroup apiPalApi + */ +static inline void convert_64_bit_to_byte_array(uint64_t value, uint8_t *data) +{ + uint8_t val_index = 0; + + while (val_index < 8) + { + data[val_index++] = value & 0xFF; + value >>= 8; + } +} + +/** + * @brief Converts a 16-Bit value into a 2 Byte array + * + * @param[in] value 16-Bit value + * @param[out] data Pointer to the 2 Byte array to be updated with 16-Bit value + * @ingroup apiPalApi + */ +static inline void convert_16_bit_to_byte_array(uint16_t value, uint8_t *data) +{ + data[0] = value & 0xFF; + data[1] = (value >> 8) & 0xFF; +} + +/* Converts a 16-Bit value into a 2 Byte array */ +static inline void convert_spec_16_bit_to_byte_array(uint16_t value, uint8_t *data) +{ + data[0] = value & 0xFF; + data[1] = (value >> 8) & 0xFF; +} + +/* Converts a 16-Bit value into a 2 Byte array */ +static inline void convert_16_bit_to_byte_address(uint16_t value, uint8_t *data) +{ + data[0] = value & 0xFF; + data[1] = (value >> 8) & 0xFF; +} + +/* + * @brief Converts a 2 Byte array into a 16-Bit value + * + * @param data Specifies the pointer to the 2 Byte array + * + * @return 16-Bit value + * @ingroup apiPalApi + */ +static inline uint16_t convert_byte_array_to_16_bit(uint8_t *data) +{ + return (data[0] | ((uint16_t)data[1] << 8)); +} + +/* Converts a 8 Byte array into a 32-Bit value */ +static inline uint32_t convert_byte_array_to_32_bit(uint8_t *data) +{ + union + { + uint32_t u32; + uint8_t u8[8]; + }long_addr; + uint8_t index; + for (index = 0; index < 4; index++) { + long_addr.u8[index] = *data++; + } + return long_addr.u32; +} + +/** + * @brief Converts a 8 Byte array into a 64-Bit value + * + * @param data Specifies the pointer to the 8 Byte array + * + * @return 64-Bit value + * @ingroup apiPalApi + */ +static inline uint64_t convert_byte_array_to_64_bit(uint8_t *data) +{ + union + { + uint64_t u64; + uint8_t u8[8]; + } long_addr; + + uint8_t val_index; + + for (val_index = 0; val_index < 8; val_index++) + { + long_addr.u8[val_index] = *data++; + } + + return long_addr.u64; +} +/** + * \} + */ + +#endif /* UTILS_COMPILER_H */ diff --git a/src/HAL/DUE/usb/conf_access.h b/src/HAL/DUE/usb/conf_access.h new file mode 100644 index 0000000..f401685 --- /dev/null +++ b/src/HAL/DUE/usb/conf_access.h @@ -0,0 +1,116 @@ +/** + * \file + * + * \brief Memory access control configuration file. + * + * Copyright (c) 2012-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _CONF_ACCESS_H_ +#define _CONF_ACCESS_H_ + +#include "compiler.h" +#include "../../../inc/MarlinConfigPre.h" + +/*! \name Activation of Logical Unit Numbers + */ +//! @{ + +#define LUN_0 ENABLE //!< SD/MMC Card over MCI Slot 0. +#define LUN_1 DISABLE +#define LUN_2 DISABLE +#define LUN_3 DISABLE +#define LUN_4 DISABLE +#define LUN_5 DISABLE +#define LUN_6 DISABLE +#define LUN_7 DISABLE +#define LUN_USB DISABLE +//! @} + +/*! \name LUN 0 Definitions + */ +//! @{ +#define SD_MMC_SPI_MEM LUN_0 +#define LUN_ID_SD_MMC_SPI_MEM LUN_ID_0 +#define LUN_0_INCLUDE "sd_mmc_spi_mem.h" +#define Lun_0_test_unit_ready sd_mmc_spi_test_unit_ready +#define Lun_0_read_capacity sd_mmc_spi_read_capacity +#define Lun_0_unload sd_mmc_spi_unload +#define Lun_0_wr_protect sd_mmc_spi_wr_protect +#define Lun_0_removal sd_mmc_spi_removal +#define Lun_0_usb_read_10 sd_mmc_spi_usb_read_10 +#define Lun_0_usb_write_10 sd_mmc_spi_usb_write_10 +#define LUN_0_NAME "\"SD/MMC Card\"" +//! @} + + +/*! \name Actions Associated with Memory Accesses + * + * Write here the action to associate with each memory access. + * + * \warning Be careful not to waste time in order not to disturb the functions. + */ +//! @{ +#define memory_start_read_action(nb_sectors) +#define memory_stop_read_action() +#define memory_start_write_action(nb_sectors) +#define memory_stop_write_action() +//! @} + +/*! \name Activation of Interface Features + */ +//! @{ +#define ACCESS_USB true //!< MEM <-> USB interface. +#define ACCESS_MEM_TO_RAM false //!< MEM <-> RAM interface. +#define ACCESS_STREAM false //!< Streaming MEM <-> MEM interface. +#define ACCESS_STREAM_RECORD false //!< Streaming MEM <-> MEM interface in record mode. +#define ACCESS_MEM_TO_MEM false //!< MEM <-> MEM interface. +#define ACCESS_CODEC false //!< Codec interface. +//! @} + +/*! \name Specific Options for Access Control + */ +//! @{ +#define GLOBAL_WR_PROTECT false //!< Management of a global write protection. +//! @} + + +#endif // _CONF_ACCESS_H_ diff --git a/src/HAL/DUE/usb/conf_clock.h b/src/HAL/DUE/usb/conf_clock.h new file mode 100644 index 0000000..97e70e9 --- /dev/null +++ b/src/HAL/DUE/usb/conf_clock.h @@ -0,0 +1,100 @@ +/** + * \file + * + * \brief SAM3X clock configuration. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef CONF_CLOCK_H_INCLUDED +#define CONF_CLOCK_H_INCLUDED + +// ===== System Clock (MCK) Source Options +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_SLCK_RC +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_SLCK_XTAL +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_SLCK_BYPASS +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_4M_RC +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_8M_RC +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_12M_RC +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_XTAL +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_BYPASS +#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_PLLACK +//#define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_UPLLCK + +// ===== System Clock (MCK) Prescaler Options (Fmck = Fsys / (SYSCLK_PRES)) +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_1 +#define CONFIG_SYSCLK_PRES SYSCLK_PRES_2 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_4 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_8 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_16 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_32 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_64 +//#define CONFIG_SYSCLK_PRES SYSCLK_PRES_3 + +// ===== PLL0 (A) Options (Fpll = (Fclk * PLL_mul) / PLL_div) +// Use mul and div effective values here. +#define CONFIG_PLL0_SOURCE PLL_SRC_MAINCK_XTAL +#define CONFIG_PLL0_MUL 14 +#define CONFIG_PLL0_DIV 1 + +// ===== UPLL (UTMI) Hardware fixed at 480MHz. + +// ===== USB Clock Source Options (Fusb = FpllX / USB_div) +// Use div effective value here. +//#define CONFIG_USBCLK_SOURCE USBCLK_SRC_PLL0 +#define CONFIG_USBCLK_SOURCE USBCLK_SRC_UPLL +#define CONFIG_USBCLK_DIV 1 + +// ===== Target frequency (System clock) +// - XTAL frequency: 12MHz +// - System clock source: PLLA +// - System clock prescaler: 2 (divided by 2) +// - PLLA source: XTAL +// - PLLA output: XTAL * 14 / 1 +// - System clock is: 12 * 14 / 1 /2 = 84MHz +// ===== Target frequency (USB Clock) +// - USB clock source: UPLL +// - USB clock divider: 1 (not divided) +// - UPLL frequency: 480MHz +// - USB clock: 480 / 1 = 480MHz + + +#endif /* CONF_CLOCK_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/conf_usb.h b/src/HAL/DUE/usb/conf_usb.h new file mode 100644 index 0000000..4de9e34 --- /dev/null +++ b/src/HAL/DUE/usb/conf_usb.h @@ -0,0 +1,296 @@ +/** + * \file + * + * \brief USB configuration file + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _CONF_USB_H_ +#define _CONF_USB_H_ + +#undef UNUSED /* To avoid a macro clash as macros.h already defines it */ +#include "../../../inc/MarlinConfigPre.h" +#include "compiler.h" + +/** + * USB Device Configuration + * @{ + */ + +//! Device definition (mandatory) +#define USB_DEVICE_MAJOR_VERSION 1 +#define USB_DEVICE_MINOR_VERSION 0 +#define USB_DEVICE_POWER 100 // Consumption on Vbus line (mA) +#define USB_DEVICE_ATTR \ + (USB_CONFIG_ATTR_SELF_POWERED) +// (USB_CONFIG_ATTR_BUS_POWERED) +// (USB_CONFIG_ATTR_REMOTE_WAKEUP|USB_CONFIG_ATTR_SELF_POWERED) +// (USB_CONFIG_ATTR_REMOTE_WAKEUP|USB_CONFIG_ATTR_BUS_POWERED) + +/** + * Device speeds support + * Low speed not supported by CDC and MSC + * @{ + */ + +//! To define a Low speed device +//#define USB_DEVICE_LOW_SPEED + +//! To define a Full speed device +//#define USB_DEVICE_FULL_SPEED + +//! To authorize the High speed +#ifndef USB_DEVICE_FULL_SPEED + #if (UC3A3||UC3A4) + #define USB_DEVICE_HS_SUPPORT + #elif (SAM3XA||SAM3U) + #define USB_DEVICE_HS_SUPPORT + #endif +#endif +//@} + + +/** + * USB Device Callbacks definitions (Optional) + * @{ + */ +#define UDC_VBUS_EVENT(b_vbus_high) +#define UDC_SOF_EVENT() +#define UDC_SUSPEND_EVENT() +#define UDC_RESUME_EVENT() +#define UDC_GET_EXTRA_STRING() usb_task_extra_string() +#define USB_DEVICE_SPECIFIC_REQUEST() usb_task_other_requests() +//@} + +#if ENABLED(SDSUPPORT) + /** + * USB Device low level configuration + * When only one interface is used, these configurations are defined by the class module. + * For composite device, these configuration must be defined here + * @{ + */ + //! Control endpoint size + #define USB_DEVICE_EP_CTRL_SIZE 64 + + //! Two interfaces for this device (CDC COM + CDC DATA + MSC) + #define USB_DEVICE_NB_INTERFACE 3 + + //! 5 endpoints used by CDC and MSC interfaces + #if SAM3U + // (3 | USB_EP_DIR_IN) // CDC Notify endpoint + // (6 | USB_EP_DIR_IN) // CDC TX + // (5 | USB_EP_DIR_OUT) // CDC RX + // (1 | USB_EP_DIR_IN) // MSC IN + // (2 | USB_EP_DIR_OUT) // MSC OUT + # define USB_DEVICE_MAX_EP 6 + # if defined(USB_DEVICE_HS_SUPPORT) + // In HS mode, size of bulk endpoints are 512 + // If CDC and MSC endpoints all uses 2 banks, DPRAM is not enough: 4 bulk + // endpoints requires 4K bytes. So reduce the number of banks of CDC bulk + // endpoints to use less DPRAM. Keep MSC setting to keep MSC performance. + # define UDD_BULK_NB_BANK(ep) ((ep == 5 || ep== 6) ? 1 : 2) + #endif + #else + // (3 | USB_EP_DIR_IN) // CDC Notify endpoint + // (4 | USB_EP_DIR_IN) // CDC TX + // (5 | USB_EP_DIR_OUT) // CDC RX + // (1 | USB_EP_DIR_IN) // MSC IN + // (2 | USB_EP_DIR_OUT) // MSC OUT + # define USB_DEVICE_MAX_EP 5 + # if SAM3XA && defined(USB_DEVICE_HS_SUPPORT) + // In HS mode, size of bulk endpoints are 512 + // If CDC and MSC endpoints all uses 2 banks, DPRAM is not enough: 4 bulk + // endpoints requires 4K bytes. So reduce the number of banks of CDC bulk + // endpoints to use less DPRAM. Keep MSC setting to keep MSC performance. + # define UDD_BULK_NB_BANK(ep) ((ep == 4 || ep== 5) ? 1 : 2) + # endif + #endif +#endif + +//@} + +//@} + + +/** + * USB Interface Configuration + * @{ + */ +/** + * Configuration of CDC interface + * @{ + */ + +//! Define one USB communication ports +#define UDI_CDC_PORT_NB 1 + +//! Interface callback definition +#define UDI_CDC_ENABLE_EXT(port) usb_task_cdc_enable(port) +#define UDI_CDC_DISABLE_EXT(port) usb_task_cdc_disable(port) +#define UDI_CDC_RX_NOTIFY(port) usb_task_cdc_rx_notify(port) +#define UDI_CDC_TX_EMPTY_NOTIFY(port) +#define UDI_CDC_SET_CODING_EXT(port,cfg) usb_task_cdc_config(port,cfg) +#define UDI_CDC_SET_DTR_EXT(port,set) usb_task_cdc_set_dtr(port,set) +#define UDI_CDC_SET_RTS_EXT(port,set) + +//! Define it when the transfer CDC Device to Host is a low rate (<512000 bauds) +//! to reduce CDC buffers size +//#define UDI_CDC_LOW_RATE + +//! Default configuration of communication port +#define UDI_CDC_DEFAULT_RATE 115200 +#define UDI_CDC_DEFAULT_STOPBITS CDC_STOP_BITS_1 +#define UDI_CDC_DEFAULT_PARITY CDC_PAR_NONE +#define UDI_CDC_DEFAULT_DATABITS 8 + +//! Enable id string of interface to add an extra USB string +#define UDI_CDC_IAD_STRING_ID 4 + +#if ENABLED(SDSUPPORT) + /** + * USB CDC low level configuration + * In standalone these configurations are defined by the CDC module. + * For composite device, these configuration must be defined here + * @{ + */ + //! Endpoint numbers definition + #if SAM3U + # define UDI_CDC_COMM_EP_0 (3 | USB_EP_DIR_IN) // Notify endpoint + # define UDI_CDC_DATA_EP_IN_0 (6 | USB_EP_DIR_IN) // TX + # define UDI_CDC_DATA_EP_OUT_0 (5 | USB_EP_DIR_OUT)// RX + #else + # define UDI_CDC_COMM_EP_0 (3 | USB_EP_DIR_IN) // Notify endpoint + # define UDI_CDC_DATA_EP_IN_0 (4 | USB_EP_DIR_IN) // TX + # define UDI_CDC_DATA_EP_OUT_0 (5 | USB_EP_DIR_OUT)// RX + #endif + + //! Interface numbers + #define UDI_CDC_COMM_IFACE_NUMBER_0 0 + #define UDI_CDC_DATA_IFACE_NUMBER_0 1 + + //@} + //@} + + + /** + * Configuration of MSC interface + * @{ + */ + //! Vendor name and Product version of MSC interface + #define UDI_MSC_GLOBAL_VENDOR_ID \ + 'M', 'A', 'R', 'L', 'I', 'N', '3', 'D' + #define UDI_MSC_GLOBAL_PRODUCT_VERSION \ + '1', '.', '0', '0' + + //! Interface callback definition + #define UDI_MSC_ENABLE_EXT() usb_task_msc_enable() + #define UDI_MSC_DISABLE_EXT() usb_task_msc_disable() + + //! Enable id string of interface to add an extra USB string + #define UDI_MSC_STRING_ID 5 + + /** + * USB MSC low level configuration + * In standalone these configurations are defined by the MSC module. + * For composite device, these configuration must be defined here + * @{ + */ + //! Endpoint numbers definition + #define UDI_MSC_EP_IN (1 | USB_EP_DIR_IN) + #define UDI_MSC_EP_OUT (2 | USB_EP_DIR_OUT) + + //! Interface number + #define UDI_MSC_IFACE_NUMBER 2 + //@} + //@} + + //@} + + + /** + * Description of Composite Device + * @{ + */ + //! USB Interfaces descriptor structure + #define UDI_COMPOSITE_DESC_T \ + usb_iad_desc_t udi_cdc_iad; \ + udi_cdc_comm_desc_t udi_cdc_comm; \ + udi_cdc_data_desc_t udi_cdc_data; \ + udi_msc_desc_t udi_msc + + //! USB Interfaces descriptor value for Full Speed + #define UDI_COMPOSITE_DESC_FS \ + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_FS, \ + .udi_msc = UDI_MSC_DESC_FS + + //! USB Interfaces descriptor value for High Speed + #define UDI_COMPOSITE_DESC_HS \ + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_HS, \ + .udi_msc = UDI_MSC_DESC_HS + + //! USB Interface APIs + #define UDI_COMPOSITE_API \ + &udi_api_cdc_comm, \ + &udi_api_cdc_data, \ + &udi_api_msc + //@} + + /** + * USB Device Driver Configuration + * @{ + */ + //@} + + //! The includes of classes and other headers must be done at the end of this file to avoid compile error + #include "udi_cdc.h" + #include "udi_msc.h" +#else + #include "udi_cdc_conf.h" +#endif + +#include "usb_task.h" + +#endif // _CONF_USB_H_ diff --git a/src/HAL/DUE/usb/ctrl_access.c b/src/HAL/DUE/usb/ctrl_access.c new file mode 100644 index 0000000..99f97f6 --- /dev/null +++ b/src/HAL/DUE/usb/ctrl_access.c @@ -0,0 +1,647 @@ +/***************************************************************************** + * + * \file + * + * \brief Abstraction layer for memory interfaces. + * + * This module contains the interfaces: + * - MEM <-> USB; + * - MEM <-> RAM; + * - MEM <-> MEM. + * + * This module may be configured and expanded to support the following features: + * - write-protected globals; + * - password-protected data; + * - specific features; + * - etc. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + ******************************************************************************/ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +//_____ I N C L U D E S ____________________________________________________ + +#include "compiler.h" +#include "preprocessor.h" +#ifdef FREERTOS_USED +#include "FreeRTOS.h" +#include "semphr.h" +#endif +#include "ctrl_access.h" + + +//_____ D E F I N I T I O N S ______________________________________________ + +#ifdef FREERTOS_USED + +/*! \name LUN Access Protection Macros + */ +//! @{ + +/*! \brief Locks accesses to LUNs. + * + * \return \c true if the access was successfully locked, else \c false. + */ +#define Ctrl_access_lock() ctrl_access_lock() + +/*! \brief Unlocks accesses to LUNs. + */ +#define Ctrl_access_unlock() xSemaphoreGive(ctrl_access_semphr) + +//! @} + +//! Handle to the semaphore protecting accesses to LUNs. +static xSemaphoreHandle ctrl_access_semphr = NULL; + +#else + +/*! \name LUN Access Protection Macros + */ +//! @{ + +/*! \brief Locks accesses to LUNs. + * + * \return \c true if the access was successfully locked, else \c false. + */ +#define Ctrl_access_lock() true + +/*! \brief Unlocks accesses to LUNs. + */ +#define Ctrl_access_unlock() + +//! @} + +#endif // FREERTOS_USED + + +#if MAX_LUN + +/*! \brief Initializes an entry of the LUN descriptor table. + * + * \param lun Logical Unit Number. + * + * \return LUN descriptor table entry initializer. + */ +#if ACCESS_USB == true && ACCESS_MEM_TO_RAM == true +#define Lun_desc_entry(lun) \ + {\ + TPASTE3(Lun_, lun, _test_unit_ready),\ + TPASTE3(Lun_, lun, _read_capacity),\ + TPASTE3(Lun_, lun, _unload),\ + TPASTE3(Lun_, lun, _wr_protect),\ + TPASTE3(Lun_, lun, _removal),\ + TPASTE3(Lun_, lun, _usb_read_10),\ + TPASTE3(Lun_, lun, _usb_write_10),\ + TPASTE3(Lun_, lun, _mem_2_ram),\ + TPASTE3(Lun_, lun, _ram_2_mem),\ + TPASTE3(LUN_, lun, _NAME)\ + } +#elif ACCESS_USB == true +#define Lun_desc_entry(lun) \ + {\ + TPASTE3(Lun_, lun, _test_unit_ready),\ + TPASTE3(Lun_, lun, _read_capacity),\ + TPASTE3(Lun_, lun, _unload),\ + TPASTE3(Lun_, lun, _wr_protect),\ + TPASTE3(Lun_, lun, _removal),\ + TPASTE3(Lun_, lun, _usb_read_10),\ + TPASTE3(Lun_, lun, _usb_write_10),\ + TPASTE3(LUN_, lun, _NAME)\ + } +#elif ACCESS_MEM_TO_RAM == true +#define Lun_desc_entry(lun) \ + {\ + TPASTE3(Lun_, lun, _test_unit_ready),\ + TPASTE3(Lun_, lun, _read_capacity),\ + TPASTE3(Lun_, lun, _unload),\ + TPASTE3(Lun_, lun, _wr_protect),\ + TPASTE3(Lun_, lun, _removal),\ + TPASTE3(Lun_, lun, _mem_2_ram),\ + TPASTE3(Lun_, lun, _ram_2_mem),\ + TPASTE3(LUN_, lun, _NAME)\ + } +#else +#define Lun_desc_entry(lun) \ + {\ + TPASTE3(Lun_, lun, _test_unit_ready),\ + TPASTE3(Lun_, lun, _read_capacity),\ + TPASTE3(Lun_, lun, _unload),\ + TPASTE3(Lun_, lun, _wr_protect),\ + TPASTE3(Lun_, lun, _removal),\ + TPASTE3(LUN_, lun, _NAME)\ + } +#endif + +//! LUN descriptor table. +static const struct +{ + Ctrl_status (*test_unit_ready)(void); + Ctrl_status (*read_capacity)(U32 *); + bool (*unload)(bool); + bool (*wr_protect)(void); + bool (*removal)(void); +#if ACCESS_USB == true + Ctrl_status (*usb_read_10)(U32, U16); + Ctrl_status (*usb_write_10)(U32, U16); +#endif +#if ACCESS_MEM_TO_RAM == true + Ctrl_status (*mem_2_ram)(U32, void *); + Ctrl_status (*ram_2_mem)(U32, const void *); +#endif + const char *name; +} lun_desc[MAX_LUN] = +{ +#if LUN_0 == ENABLE +# ifndef Lun_0_unload +# define Lun_0_unload NULL +# endif + Lun_desc_entry(0), +#endif +#if LUN_1 == ENABLE +# ifndef Lun_1_unload +# define Lun_1_unload NULL +# endif + Lun_desc_entry(1), +#endif +#if LUN_2 == ENABLE +# ifndef Lun_2_unload +# define Lun_2_unload NULL +# endif + Lun_desc_entry(2), +#endif +#if LUN_3 == ENABLE +# ifndef Lun_3_unload +# define Lun_3_unload NULL +# endif + Lun_desc_entry(3), +#endif +#if LUN_4 == ENABLE +# ifndef Lun_4_unload +# define Lun_4_unload NULL +# endif + Lun_desc_entry(4), +#endif +#if LUN_5 == ENABLE +# ifndef Lun_5_unload +# define Lun_5_unload NULL +# endif + Lun_desc_entry(5), +#endif +#if LUN_6 == ENABLE +# ifndef Lun_6_unload +# define Lun_6_unload NULL +# endif + Lun_desc_entry(6), +#endif +#if LUN_7 == ENABLE +# ifndef Lun_7_unload +# define Lun_7_unload NULL +# endif + Lun_desc_entry(7) +#endif +}; + +#endif + + +#if GLOBAL_WR_PROTECT == true +bool g_wr_protect; +#endif + + +/*! \name Control Interface + */ +//! @{ + + +#ifdef FREERTOS_USED + +bool ctrl_access_init(void) +{ + // If the handle to the protecting semaphore is not valid, + if (!ctrl_access_semphr) + { + // try to create the semaphore. + vSemaphoreCreateBinary(ctrl_access_semphr); + + // If the semaphore could not be created, there is no backup solution. + if (!ctrl_access_semphr) return false; + } + + return true; +} + + +/*! \brief Locks accesses to LUNs. + * + * \return \c true if the access was successfully locked, else \c false. + */ +static bool ctrl_access_lock(void) +{ + // If the semaphore could not be created, there is no backup solution. + if (!ctrl_access_semphr) return false; + + // Wait for the semaphore. + while (!xSemaphoreTake(ctrl_access_semphr, portMAX_DELAY)); + + return true; +} + +#endif // FREERTOS_USED + + +U8 get_nb_lun(void) +{ +#if MEM_USB == ENABLE +# ifndef Lun_usb_get_lun +# define Lun_usb_get_lun() host_get_lun() +# endif + U8 nb_lun; + + if (!Ctrl_access_lock()) return MAX_LUN; + + nb_lun = MAX_LUN + Lun_usb_get_lun(); + + Ctrl_access_unlock(); + + return nb_lun; +#else + return MAX_LUN; +#endif +} + + +U8 get_cur_lun(void) +{ + return LUN_ID_0; +} + + +Ctrl_status mem_test_unit_ready(U8 lun) +{ + Ctrl_status status; + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].test_unit_ready() : +#endif +#if LUN_USB == ENABLE + Lun_usb_test_unit_ready(lun - LUN_ID_USB); +#else + CTRL_FAIL; +#endif + + Ctrl_access_unlock(); + + return status; +} + + +Ctrl_status mem_read_capacity(U8 lun, U32 *u32_nb_sector) +{ + Ctrl_status status; + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].read_capacity(u32_nb_sector) : +#endif +#if LUN_USB == ENABLE + Lun_usb_read_capacity(lun - LUN_ID_USB, u32_nb_sector); +#else + CTRL_FAIL; +#endif + + Ctrl_access_unlock(); + + return status; +} + + +U8 mem_sector_size(U8 lun) +{ + U8 sector_size; + + if (!Ctrl_access_lock()) return 0; + + sector_size = +#if MAX_LUN + (lun < MAX_LUN) ? 1 : +#endif +#if LUN_USB == ENABLE + Lun_usb_read_sector_size(lun - LUN_ID_USB); +#else + 0; +#endif + + Ctrl_access_unlock(); + + return sector_size; +} + + +bool mem_unload(U8 lun, bool unload) +{ + bool unloaded; +#if !MAX_LUN || !defined(Lun_usb_unload) + UNUSED(lun); +#endif + + if (!Ctrl_access_lock()) return false; + + unloaded = +#if MAX_LUN + (lun < MAX_LUN) ? + (lun_desc[lun].unload ? + lun_desc[lun].unload(unload) : !unload) : +#endif +#if LUN_USB == ENABLE +# if defined(Lun_usb_unload) + Lun_usb_unload(lun - LUN_ID_USB, unload); +# else + !unload; /* Can not unload: load success, unload fail */ +# endif +#else + false; /* No mem, unload/load fail */ +#endif + + Ctrl_access_unlock(); + + return unloaded; +} + +bool mem_wr_protect(U8 lun) +{ + bool wr_protect; + + if (!Ctrl_access_lock()) return true; + + wr_protect = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].wr_protect() : +#endif +#if LUN_USB == ENABLE + Lun_usb_wr_protect(lun - LUN_ID_USB); +#else + true; +#endif + + Ctrl_access_unlock(); + + return wr_protect; +} + + +bool mem_removal(U8 lun) +{ + bool removal; +#if MAX_LUN==0 + UNUSED(lun); +#endif + + if (!Ctrl_access_lock()) return true; + + removal = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].removal() : +#endif +#if LUN_USB == ENABLE + Lun_usb_removal(); +#else + true; +#endif + + Ctrl_access_unlock(); + + return removal; +} + + +const char *mem_name(U8 lun) +{ +#if MAX_LUN==0 + UNUSED(lun); +#endif + return +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].name : +#endif +#if LUN_USB == ENABLE + LUN_USB_NAME; +#else + NULL; +#endif +} + + +//! @} + + +#if ACCESS_USB == true + +/*! \name MEM <-> USB Interface + */ +//! @{ + + +Ctrl_status memory_2_usb(U8 lun, U32 addr, U16 nb_sector) +{ + Ctrl_status status; + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + memory_start_read_action(nb_sector); + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].usb_read_10(addr, nb_sector) : +#endif + CTRL_FAIL; + memory_stop_read_action(); + + Ctrl_access_unlock(); + + return status; +} + + +Ctrl_status usb_2_memory(U8 lun, U32 addr, U16 nb_sector) +{ + Ctrl_status status; + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + memory_start_write_action(nb_sector); + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].usb_write_10(addr, nb_sector) : +#endif + CTRL_FAIL; + memory_stop_write_action(); + + Ctrl_access_unlock(); + + return status; +} + + +//! @} + +#endif // ACCESS_USB == true + + +#if ACCESS_MEM_TO_RAM == true + +/*! \name MEM <-> RAM Interface + */ +//! @{ + + +Ctrl_status memory_2_ram(U8 lun, U32 addr, void *ram) +{ + Ctrl_status status; +#if MAX_LUN==0 + UNUSED(lun); +#endif + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + memory_start_read_action(1); + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].mem_2_ram(addr, ram) : +#endif +#if LUN_USB == ENABLE + Lun_usb_mem_2_ram(addr, ram); +#else + CTRL_FAIL; +#endif + memory_stop_read_action(); + + Ctrl_access_unlock(); + + return status; +} + + +Ctrl_status ram_2_memory(U8 lun, U32 addr, const void *ram) +{ + Ctrl_status status; +#if MAX_LUN==0 + UNUSED(lun); +#endif + + if (!Ctrl_access_lock()) return CTRL_FAIL; + + memory_start_write_action(1); + status = +#if MAX_LUN + (lun < MAX_LUN) ? lun_desc[lun].ram_2_mem(addr, ram) : +#endif +#if LUN_USB == ENABLE + Lun_usb_ram_2_mem(addr, ram); +#else + CTRL_FAIL; +#endif + memory_stop_write_action(); + + Ctrl_access_unlock(); + + return status; +} + + +//! @} + +#endif // ACCESS_MEM_TO_RAM == true + + +#if ACCESS_STREAM == true + +/*! \name Streaming MEM <-> MEM Interface + */ +//! @{ + + + #if ACCESS_MEM_TO_MEM == true + +#include "fat.h" + +Ctrl_status stream_mem_to_mem(U8 src_lun, U32 src_addr, U8 dest_lun, U32 dest_addr, U16 nb_sector) +{ + COMPILER_ALIGNED(4) + static U8 sector_buf[FS_512B]; + Ctrl_status status = CTRL_GOOD; + + while (nb_sector--) + { + if ((status = memory_2_ram(src_lun, src_addr++, sector_buf)) != CTRL_GOOD) break; + if ((status = ram_2_memory(dest_lun, dest_addr++, sector_buf)) != CTRL_GOOD) break; + } + + return status; +} + + #endif // ACCESS_MEM_TO_MEM == true + + +Ctrl_status stream_state(U8 id) +{ + UNUSED(id); + return CTRL_GOOD; +} + + +U16 stream_stop(U8 id) +{ + UNUSED(id); + return 0; +} + + +//! @} + +#endif // ACCESS_STREAM + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/ctrl_access.h b/src/HAL/DUE/usb/ctrl_access.h new file mode 100644 index 0000000..b338390 --- /dev/null +++ b/src/HAL/DUE/usb/ctrl_access.h @@ -0,0 +1,402 @@ +/***************************************************************************** + * + * \file + * + * \brief Abstraction layer for memory interfaces. + * + * This module contains the interfaces: + * - MEM <-> USB; + * - MEM <-> RAM; + * - MEM <-> MEM. + * + * This module may be configured and expanded to support the following features: + * - write-protected globals; + * - password-protected data; + * - specific features; + * - etc. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + ******************************************************************************/ +/* + * Support and FAQ: visit Atmel Support + */ + + +#ifndef _CTRL_ACCESS_H_ +#define _CTRL_ACCESS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \defgroup group_common_services_storage_ctrl_access Memory Control Access + * + * Common abstraction layer for memory interfaces. It provides interfaces between: + * Memory and USB, Memory and RAM, Memory and Memory. Common API for XMEGA and UC3. + * + * \{ + */ + +#include "compiler.h" +#include "conf_access.h" + +#ifndef SECTOR_SIZE +#define SECTOR_SIZE 512 +#endif + +//! Status returned by CTRL_ACCESS interfaces. +typedef enum +{ + CTRL_GOOD = PASS, //!< Success, memory ready. + CTRL_FAIL = FAIL, //!< An error occurred. + CTRL_NO_PRESENT = FAIL + 1, //!< Memory unplugged. + CTRL_BUSY = FAIL + 2 //!< Memory not initialized or changed. +} Ctrl_status; + + +// FYI: Each Logical Unit Number (LUN) corresponds to a memory. + +// Check LUN defines. +#ifndef LUN_0 + #error LUN_0 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_1 + #error LUN_1 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_2 + #error LUN_2 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_3 + #error LUN_3 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_4 + #error LUN_4 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_5 + #error LUN_5 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_6 + #error LUN_6 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_7 + #error LUN_7 must be defined as ENABLE or DISABLE in conf_access.h +#endif +#ifndef LUN_USB + #error LUN_USB must be defined as ENABLE or DISABLE in conf_access.h +#endif + +/*! \name LUN IDs + */ +//! @{ +#define LUN_ID_0 (0) //!< First static LUN. +#define LUN_ID_1 (LUN_ID_0 + LUN_0) +#define LUN_ID_2 (LUN_ID_1 + LUN_1) +#define LUN_ID_3 (LUN_ID_2 + LUN_2) +#define LUN_ID_4 (LUN_ID_3 + LUN_3) +#define LUN_ID_5 (LUN_ID_4 + LUN_4) +#define LUN_ID_6 (LUN_ID_5 + LUN_5) +#define LUN_ID_7 (LUN_ID_6 + LUN_6) +#define MAX_LUN (LUN_ID_7 + LUN_7) //!< Number of static LUNs. +#define LUN_ID_USB (MAX_LUN) //!< First dynamic LUN (USB host mass storage). +//! @} + + +// Include LUN header files. +#if LUN_0 == ENABLE + #include LUN_0_INCLUDE +#endif +#if LUN_1 == ENABLE + #include LUN_1_INCLUDE +#endif +#if LUN_2 == ENABLE + #include LUN_2_INCLUDE +#endif +#if LUN_3 == ENABLE + #include LUN_3_INCLUDE +#endif +#if LUN_4 == ENABLE + #include LUN_4_INCLUDE +#endif +#if LUN_5 == ENABLE + #include LUN_5_INCLUDE +#endif +#if LUN_6 == ENABLE + #include LUN_6_INCLUDE +#endif +#if LUN_7 == ENABLE + #include LUN_7_INCLUDE +#endif +#if LUN_USB == ENABLE + #include LUN_USB_INCLUDE +#endif + + +// Check the configuration of write protection in conf_access.h. +#ifndef GLOBAL_WR_PROTECT + #error GLOBAL_WR_PROTECT must be defined as true or false in conf_access.h +#endif + + +#if GLOBAL_WR_PROTECT == true + +//! Write protect. +extern bool g_wr_protect; + +#endif + + +/*! \name Control Interface + */ +//! @{ + +#ifdef FREERTOS_USED + +/*! \brief Initializes the LUN access locker. + * + * \return \c true if the locker was successfully initialized, else \c false. + */ +extern bool ctrl_access_init(void); + +#endif // FREERTOS_USED + +/*! \brief Returns the number of LUNs. + * + * \return Number of LUNs in the system. + */ +extern U8 get_nb_lun(void); + +/*! \brief Returns the current LUN. + * + * \return Current LUN. + * + * \todo Implement. + */ +extern U8 get_cur_lun(void); + +/*! \brief Tests the memory state and initializes the memory if required. + * + * The TEST UNIT READY SCSI primary command allows an application client to poll + * a LUN until it is ready without having to allocate memory for returned data. + * + * This command may be used to check the media status of LUNs with removable + * media. + * + * \param lun Logical Unit Number. + * + * \return Status. + */ +extern Ctrl_status mem_test_unit_ready(U8 lun); + +/*! \brief Returns the address of the last valid sector (512 bytes) in the + * memory. + * + * \param lun Logical Unit Number. + * \param u32_nb_sector Pointer to the address of the last valid sector. + * + * \return Status. + */ +extern Ctrl_status mem_read_capacity(U8 lun, U32 *u32_nb_sector); + +/*! \brief Returns the size of the physical sector. + * + * \param lun Logical Unit Number. + * + * \return Sector size (unit: 512 bytes). + */ +extern U8 mem_sector_size(U8 lun); + +/*! \brief Unload/load the medium. + * + * \param lun Logical Unit Number. + * \param unload \c true to unload the medium, \c false to load the medium. + * + * \return \c true if unload/load success, else \c false. + */ +extern bool mem_unload(U8 lun, bool unload); + +/*! \brief Returns the write-protection state of the memory. + * + * \param lun Logical Unit Number. + * + * \return \c true if the memory is write-protected, else \c false. + * + * \note Only used by removable memories with hardware-specific write + * protection. + */ +extern bool mem_wr_protect(U8 lun); + +/*! \brief Tells whether the memory is removable. + * + * \param lun Logical Unit Number. + * + * \return \c true if the memory is removable, else \c false. + */ +extern bool mem_removal(U8 lun); + +/*! \brief Returns a pointer to the LUN name. + * + * \param lun Logical Unit Number. + * + * \return Pointer to the LUN name string. + */ +extern const char *mem_name(U8 lun); + +//! @} + + +#if ACCESS_USB == true + +/*! \name MEM <-> USB Interface + */ +//! @{ + +/*! \brief Transfers data from the memory to USB. + * + * \param lun Logical Unit Number. + * \param addr Address of first memory sector to read. + * \param nb_sector Number of sectors to transfer. + * + * \return Status. + */ +extern Ctrl_status memory_2_usb(U8 lun, U32 addr, U16 nb_sector); + +/*! \brief Transfers data from USB to the memory. + * + * \param lun Logical Unit Number. + * \param addr Address of first memory sector to write. + * \param nb_sector Number of sectors to transfer. + * + * \return Status. + */ +extern Ctrl_status usb_2_memory(U8 lun, U32 addr, U16 nb_sector); + +//! @} + +#endif // ACCESS_USB == true + + +#if ACCESS_MEM_TO_RAM == true + +/*! \name MEM <-> RAM Interface + */ +//! @{ + +/*! \brief Copies 1 data sector from the memory to RAM. + * + * \param lun Logical Unit Number. + * \param addr Address of first memory sector to read. + * \param ram Pointer to RAM buffer to write. + * + * \return Status. + */ +extern Ctrl_status memory_2_ram(U8 lun, U32 addr, void *ram); + +/*! \brief Copies 1 data sector from RAM to the memory. + * + * \param lun Logical Unit Number. + * \param addr Address of first memory sector to write. + * \param ram Pointer to RAM buffer to read. + * + * \return Status. + */ +extern Ctrl_status ram_2_memory(U8 lun, U32 addr, const void *ram); + +//! @} + +#endif // ACCESS_MEM_TO_RAM == true + + +#if ACCESS_STREAM == true + +/*! \name Streaming MEM <-> MEM Interface + */ +//! @{ + +//! Erroneous streaming data transfer ID. +#define ID_STREAM_ERR 0xFF + + #if ACCESS_MEM_TO_MEM == true + +/*! \brief Copies data from one memory to another. + * + * \param src_lun Source Logical Unit Number. + * \param src_addr Source address of first memory sector to read. + * \param dest_lun Destination Logical Unit Number. + * \param dest_addr Destination address of first memory sector to write. + * \param nb_sector Number of sectors to copy. + * + * \return Status. + */ +extern Ctrl_status stream_mem_to_mem(U8 src_lun, U32 src_addr, U8 dest_lun, U32 dest_addr, U16 nb_sector); + + #endif // ACCESS_MEM_TO_MEM == true + +/*! \brief Returns the state of a streaming data transfer. + * + * \param id Transfer ID. + * + * \return Status. + * + * \todo Implement. + */ +extern Ctrl_status stream_state(U8 id); + +/*! \brief Stops a streaming data transfer. + * + * \param id Transfer ID. + * + * \return Number of remaining sectors. + * + * \todo Implement. + */ +extern U16 stream_stop(U8 id); + +//! @} + +#endif // ACCESS_STREAM == true + +/** + * \} + */ + +#ifdef __cplusplus +} +#endif + +#endif // _CTRL_ACCESS_H_ diff --git a/src/HAL/DUE/usb/genclk.h b/src/HAL/DUE/usb/genclk.h new file mode 100644 index 0000000..cde03bc --- /dev/null +++ b/src/HAL/DUE/usb/genclk.h @@ -0,0 +1,278 @@ +/** + * \file + * + * \brief Chip-specific generic clock management. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef CHIP_GENCLK_H_INCLUDED +#define CHIP_GENCLK_H_INCLUDED + +#include +#include + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +/** + * \weakgroup genclk_group + * @{ + */ + +//! \name Programmable Clock Identifiers (PCK) +//@{ +#define GENCLK_PCK_0 0 //!< PCK0 ID +#define GENCLK_PCK_1 1 //!< PCK1 ID +#define GENCLK_PCK_2 2 //!< PCK2 ID +//@} + +//! \name Programmable Clock Sources (PCK) +//@{ + +enum genclk_source { + GENCLK_PCK_SRC_SLCK_RC = 0, //!< Internal 32kHz RC oscillator as PCK source clock + GENCLK_PCK_SRC_SLCK_XTAL = 1, //!< External 32kHz crystal oscillator as PCK source clock + GENCLK_PCK_SRC_SLCK_BYPASS = 2, //!< External 32kHz bypass oscillator as PCK source clock + GENCLK_PCK_SRC_MAINCK_4M_RC = 3, //!< Internal 4MHz RC oscillator as PCK source clock + GENCLK_PCK_SRC_MAINCK_8M_RC = 4, //!< Internal 8MHz RC oscillator as PCK source clock + GENCLK_PCK_SRC_MAINCK_12M_RC = 5, //!< Internal 12MHz RC oscillator as PCK source clock + GENCLK_PCK_SRC_MAINCK_XTAL = 6, //!< External crystal oscillator as PCK source clock + GENCLK_PCK_SRC_MAINCK_BYPASS = 7, //!< External bypass oscillator as PCK source clock + GENCLK_PCK_SRC_PLLACK = 8, //!< Use PLLACK as PCK source clock + GENCLK_PCK_SRC_PLLBCK = 9, //!< Use PLLBCK as PCK source clock + GENCLK_PCK_SRC_MCK = 10, //!< Use Master Clk as PCK source clock +}; + +//@} + +//! \name Programmable Clock Prescalers (PCK) +//@{ + +enum genclk_divider { + GENCLK_PCK_PRES_1 = PMC_PCK_PRES_CLK_1, //!< Set PCK clock prescaler to 1 + GENCLK_PCK_PRES_2 = PMC_PCK_PRES_CLK_2, //!< Set PCK clock prescaler to 2 + GENCLK_PCK_PRES_4 = PMC_PCK_PRES_CLK_4, //!< Set PCK clock prescaler to 4 + GENCLK_PCK_PRES_8 = PMC_PCK_PRES_CLK_8, //!< Set PCK clock prescaler to 8 + GENCLK_PCK_PRES_16 = PMC_PCK_PRES_CLK_16, //!< Set PCK clock prescaler to 16 + GENCLK_PCK_PRES_32 = PMC_PCK_PRES_CLK_32, //!< Set PCK clock prescaler to 32 + GENCLK_PCK_PRES_64 = PMC_PCK_PRES_CLK_64, //!< Set PCK clock prescaler to 64 +}; + +//@} + +struct genclk_config { + uint32_t ctrl; +}; + +static inline void genclk_config_defaults(struct genclk_config *p_cfg, + uint32_t ul_id) +{ + ul_id = ul_id; + p_cfg->ctrl = 0; +} + +static inline void genclk_config_read(struct genclk_config *p_cfg, + uint32_t ul_id) +{ + p_cfg->ctrl = PMC->PMC_PCK[ul_id]; +} + +static inline void genclk_config_write(const struct genclk_config *p_cfg, + uint32_t ul_id) +{ + PMC->PMC_PCK[ul_id] = p_cfg->ctrl; +} + +//! \name Programmable Clock Source and Prescaler configuration +//@{ + +static inline void genclk_config_set_source(struct genclk_config *p_cfg, + enum genclk_source e_src) +{ + p_cfg->ctrl &= (~PMC_PCK_CSS_Msk); + + switch (e_src) { + case GENCLK_PCK_SRC_SLCK_RC: + case GENCLK_PCK_SRC_SLCK_XTAL: + case GENCLK_PCK_SRC_SLCK_BYPASS: + p_cfg->ctrl |= (PMC_PCK_CSS_SLOW_CLK); + break; + + case GENCLK_PCK_SRC_MAINCK_4M_RC: + case GENCLK_PCK_SRC_MAINCK_8M_RC: + case GENCLK_PCK_SRC_MAINCK_12M_RC: + case GENCLK_PCK_SRC_MAINCK_XTAL: + case GENCLK_PCK_SRC_MAINCK_BYPASS: + p_cfg->ctrl |= (PMC_PCK_CSS_MAIN_CLK); + break; + + case GENCLK_PCK_SRC_PLLACK: + p_cfg->ctrl |= (PMC_PCK_CSS_PLLA_CLK); + break; + + case GENCLK_PCK_SRC_PLLBCK: + p_cfg->ctrl |= (PMC_PCK_CSS_UPLL_CLK); + break; + + case GENCLK_PCK_SRC_MCK: + p_cfg->ctrl |= (PMC_PCK_CSS_MCK); + break; + } +} + +static inline void genclk_config_set_divider(struct genclk_config *p_cfg, + uint32_t e_divider) +{ + p_cfg->ctrl &= ~PMC_PCK_PRES_Msk; + p_cfg->ctrl |= e_divider; +} + +//@} + +static inline void genclk_enable(const struct genclk_config *p_cfg, + uint32_t ul_id) +{ + PMC->PMC_PCK[ul_id] = p_cfg->ctrl; + pmc_enable_pck(ul_id); +} + +static inline void genclk_disable(uint32_t ul_id) +{ + pmc_disable_pck(ul_id); +} + +static inline void genclk_enable_source(enum genclk_source e_src) +{ + switch (e_src) { + case GENCLK_PCK_SRC_SLCK_RC: + if (!osc_is_ready(OSC_SLCK_32K_RC)) { + osc_enable(OSC_SLCK_32K_RC); + osc_wait_ready(OSC_SLCK_32K_RC); + } + break; + + case GENCLK_PCK_SRC_SLCK_XTAL: + if (!osc_is_ready(OSC_SLCK_32K_XTAL)) { + osc_enable(OSC_SLCK_32K_XTAL); + osc_wait_ready(OSC_SLCK_32K_XTAL); + } + break; + + case GENCLK_PCK_SRC_SLCK_BYPASS: + if (!osc_is_ready(OSC_SLCK_32K_BYPASS)) { + osc_enable(OSC_SLCK_32K_BYPASS); + osc_wait_ready(OSC_SLCK_32K_BYPASS); + } + break; + + case GENCLK_PCK_SRC_MAINCK_4M_RC: + if (!osc_is_ready(OSC_MAINCK_4M_RC)) { + osc_enable(OSC_MAINCK_4M_RC); + osc_wait_ready(OSC_MAINCK_4M_RC); + } + break; + + case GENCLK_PCK_SRC_MAINCK_8M_RC: + if (!osc_is_ready(OSC_MAINCK_8M_RC)) { + osc_enable(OSC_MAINCK_8M_RC); + osc_wait_ready(OSC_MAINCK_8M_RC); + } + break; + + case GENCLK_PCK_SRC_MAINCK_12M_RC: + if (!osc_is_ready(OSC_MAINCK_12M_RC)) { + osc_enable(OSC_MAINCK_12M_RC); + osc_wait_ready(OSC_MAINCK_12M_RC); + } + break; + + case GENCLK_PCK_SRC_MAINCK_XTAL: + if (!osc_is_ready(OSC_MAINCK_XTAL)) { + osc_enable(OSC_MAINCK_XTAL); + osc_wait_ready(OSC_MAINCK_XTAL); + } + break; + + case GENCLK_PCK_SRC_MAINCK_BYPASS: + if (!osc_is_ready(OSC_MAINCK_BYPASS)) { + osc_enable(OSC_MAINCK_BYPASS); + osc_wait_ready(OSC_MAINCK_BYPASS); + } + break; + +#ifdef CONFIG_PLL0_SOURCE + case GENCLK_PCK_SRC_PLLACK: + pll_enable_config_defaults(0); + break; +#endif + +#ifdef CONFIG_PLL1_SOURCE + case GENCLK_PCK_SRC_PLLBCK: + pll_enable_config_defaults(1); + break; +#endif + + case GENCLK_PCK_SRC_MCK: + break; + + default: + Assert(false); + break; + } +} + +//! @} + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif /* CHIP_GENCLK_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/mrepeat.h b/src/HAL/DUE/usb/mrepeat.h new file mode 100644 index 0000000..8363d9c --- /dev/null +++ b/src/HAL/DUE/usb/mrepeat.h @@ -0,0 +1,339 @@ +/** + * \file + * + * \brief Preprocessor macro repeating utils. + * + * Copyright (c) 2010-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _MREPEAT_H_ +#define _MREPEAT_H_ + +/** + * \defgroup group_sam_utils_mrepeat Preprocessor - Macro Repeat + * + * \ingroup group_sam_utils + * + * \{ + */ + +#include "preprocessor.h" + + +//! Maximal number of repetitions supported by MREPEAT. +#define MREPEAT_LIMIT 256 + +/*! \brief Macro repeat. + * + * This macro represents a horizontal repetition construct. + * + * \param count The number of repetitious calls to macro. Valid values range from 0 to MREPEAT_LIMIT. + * \param macro A binary operation of the form macro(n, data). This macro is expanded by MREPEAT with + * the current repetition number and the auxiliary data argument. + * \param data Auxiliary data passed to macro. + * + * \return macro(0, data) macro(1, data) ... macro(count - 1, data) + */ +#define MREPEAT(count, macro, data) TPASTE2(MREPEAT, count)(macro, data) + +#define MREPEAT0( macro, data) +#define MREPEAT1( macro, data) MREPEAT0( macro, data) macro( 0, data) +#define MREPEAT2( macro, data) MREPEAT1( macro, data) macro( 1, data) +#define MREPEAT3( macro, data) MREPEAT2( macro, data) macro( 2, data) +#define MREPEAT4( macro, data) MREPEAT3( macro, data) macro( 3, data) +#define MREPEAT5( macro, data) MREPEAT4( macro, data) macro( 4, data) +#define MREPEAT6( macro, data) MREPEAT5( macro, data) macro( 5, data) +#define MREPEAT7( macro, data) MREPEAT6( macro, data) macro( 6, data) +#define MREPEAT8( macro, data) MREPEAT7( macro, data) macro( 7, data) +#define MREPEAT9( macro, data) MREPEAT8( macro, data) macro( 8, data) +#define MREPEAT10( macro, data) MREPEAT9( macro, data) macro( 9, data) +#define MREPEAT11( macro, data) MREPEAT10( macro, data) macro( 10, data) +#define MREPEAT12( macro, data) MREPEAT11( macro, data) macro( 11, data) +#define MREPEAT13( macro, data) MREPEAT12( macro, data) macro( 12, data) +#define MREPEAT14( macro, data) MREPEAT13( macro, data) macro( 13, data) +#define MREPEAT15( macro, data) MREPEAT14( macro, data) macro( 14, data) +#define MREPEAT16( macro, data) MREPEAT15( macro, data) macro( 15, data) +#define MREPEAT17( macro, data) MREPEAT16( macro, data) macro( 16, data) +#define MREPEAT18( macro, data) MREPEAT17( macro, data) macro( 17, data) +#define MREPEAT19( macro, data) MREPEAT18( macro, data) macro( 18, data) +#define MREPEAT20( macro, data) MREPEAT19( macro, data) macro( 19, data) +#define MREPEAT21( macro, data) MREPEAT20( macro, data) macro( 20, data) +#define MREPEAT22( macro, data) MREPEAT21( macro, data) macro( 21, data) +#define MREPEAT23( macro, data) MREPEAT22( macro, data) macro( 22, data) +#define MREPEAT24( macro, data) MREPEAT23( macro, data) macro( 23, data) +#define MREPEAT25( macro, data) MREPEAT24( macro, data) macro( 24, data) +#define MREPEAT26( macro, data) MREPEAT25( macro, data) macro( 25, data) +#define MREPEAT27( macro, data) MREPEAT26( macro, data) macro( 26, data) +#define MREPEAT28( macro, data) MREPEAT27( macro, data) macro( 27, data) +#define MREPEAT29( macro, data) MREPEAT28( macro, data) macro( 28, data) +#define MREPEAT30( macro, data) MREPEAT29( macro, data) macro( 29, data) +#define MREPEAT31( macro, data) MREPEAT30( macro, data) macro( 30, data) +#define MREPEAT32( macro, data) MREPEAT31( macro, data) macro( 31, data) +#define MREPEAT33( macro, data) MREPEAT32( macro, data) macro( 32, data) +#define MREPEAT34( macro, data) MREPEAT33( macro, data) macro( 33, data) +#define MREPEAT35( macro, data) MREPEAT34( macro, data) macro( 34, data) +#define MREPEAT36( macro, data) MREPEAT35( macro, data) macro( 35, data) +#define MREPEAT37( macro, data) MREPEAT36( macro, data) macro( 36, data) +#define MREPEAT38( macro, data) MREPEAT37( macro, data) macro( 37, data) +#define MREPEAT39( macro, data) MREPEAT38( macro, data) macro( 38, data) +#define MREPEAT40( macro, data) MREPEAT39( macro, data) macro( 39, data) +#define MREPEAT41( macro, data) MREPEAT40( macro, data) macro( 40, data) +#define MREPEAT42( macro, data) MREPEAT41( macro, data) macro( 41, data) +#define MREPEAT43( macro, data) MREPEAT42( macro, data) macro( 42, data) +#define MREPEAT44( macro, data) MREPEAT43( macro, data) macro( 43, data) +#define MREPEAT45( macro, data) MREPEAT44( macro, data) macro( 44, data) +#define MREPEAT46( macro, data) MREPEAT45( macro, data) macro( 45, data) +#define MREPEAT47( macro, data) MREPEAT46( macro, data) macro( 46, data) +#define MREPEAT48( macro, data) MREPEAT47( macro, data) macro( 47, data) +#define MREPEAT49( macro, data) MREPEAT48( macro, data) macro( 48, data) +#define MREPEAT50( macro, data) MREPEAT49( macro, data) macro( 49, data) +#define MREPEAT51( macro, data) MREPEAT50( macro, data) macro( 50, data) +#define MREPEAT52( macro, data) MREPEAT51( macro, data) macro( 51, data) +#define MREPEAT53( macro, data) MREPEAT52( macro, data) macro( 52, data) +#define MREPEAT54( macro, data) MREPEAT53( macro, data) macro( 53, data) +#define MREPEAT55( macro, data) MREPEAT54( macro, data) macro( 54, data) +#define MREPEAT56( macro, data) MREPEAT55( macro, data) macro( 55, data) +#define MREPEAT57( macro, data) MREPEAT56( macro, data) macro( 56, data) +#define MREPEAT58( macro, data) MREPEAT57( macro, data) macro( 57, data) +#define MREPEAT59( macro, data) MREPEAT58( macro, data) macro( 58, data) +#define MREPEAT60( macro, data) MREPEAT59( macro, data) macro( 59, data) +#define MREPEAT61( macro, data) MREPEAT60( macro, data) macro( 60, data) +#define MREPEAT62( macro, data) MREPEAT61( macro, data) macro( 61, data) +#define MREPEAT63( macro, data) MREPEAT62( macro, data) macro( 62, data) +#define MREPEAT64( macro, data) MREPEAT63( macro, data) macro( 63, data) +#define MREPEAT65( macro, data) MREPEAT64( macro, data) macro( 64, data) +#define MREPEAT66( macro, data) MREPEAT65( macro, data) macro( 65, data) +#define MREPEAT67( macro, data) MREPEAT66( macro, data) macro( 66, data) +#define MREPEAT68( macro, data) MREPEAT67( macro, data) macro( 67, data) +#define MREPEAT69( macro, data) MREPEAT68( macro, data) macro( 68, data) +#define MREPEAT70( macro, data) MREPEAT69( macro, data) macro( 69, data) +#define MREPEAT71( macro, data) MREPEAT70( macro, data) macro( 70, data) +#define MREPEAT72( macro, data) MREPEAT71( macro, data) macro( 71, data) +#define MREPEAT73( macro, data) MREPEAT72( macro, data) macro( 72, data) +#define MREPEAT74( macro, data) MREPEAT73( macro, data) macro( 73, data) +#define MREPEAT75( macro, data) MREPEAT74( macro, data) macro( 74, data) +#define MREPEAT76( macro, data) MREPEAT75( macro, data) macro( 75, data) +#define MREPEAT77( macro, data) MREPEAT76( macro, data) macro( 76, data) +#define MREPEAT78( macro, data) MREPEAT77( macro, data) macro( 77, data) +#define MREPEAT79( macro, data) MREPEAT78( macro, data) macro( 78, data) +#define MREPEAT80( macro, data) MREPEAT79( macro, data) macro( 79, data) +#define MREPEAT81( macro, data) MREPEAT80( macro, data) macro( 80, data) +#define MREPEAT82( macro, data) MREPEAT81( macro, data) macro( 81, data) +#define MREPEAT83( macro, data) MREPEAT82( macro, data) macro( 82, data) +#define MREPEAT84( macro, data) MREPEAT83( macro, data) macro( 83, data) +#define MREPEAT85( macro, data) MREPEAT84( macro, data) macro( 84, data) +#define MREPEAT86( macro, data) MREPEAT85( macro, data) macro( 85, data) +#define MREPEAT87( macro, data) MREPEAT86( macro, data) macro( 86, data) +#define MREPEAT88( macro, data) MREPEAT87( macro, data) macro( 87, data) +#define MREPEAT89( macro, data) MREPEAT88( macro, data) macro( 88, data) +#define MREPEAT90( macro, data) MREPEAT89( macro, data) macro( 89, data) +#define MREPEAT91( macro, data) MREPEAT90( macro, data) macro( 90, data) +#define MREPEAT92( macro, data) MREPEAT91( macro, data) macro( 91, data) +#define MREPEAT93( macro, data) MREPEAT92( macro, data) macro( 92, data) +#define MREPEAT94( macro, data) MREPEAT93( macro, data) macro( 93, data) +#define MREPEAT95( macro, data) MREPEAT94( macro, data) macro( 94, data) +#define MREPEAT96( macro, data) MREPEAT95( macro, data) macro( 95, data) +#define MREPEAT97( macro, data) MREPEAT96( macro, data) macro( 96, data) +#define MREPEAT98( macro, data) MREPEAT97( macro, data) macro( 97, data) +#define MREPEAT99( macro, data) MREPEAT98( macro, data) macro( 98, data) +#define MREPEAT100(macro, data) MREPEAT99( macro, data) macro( 99, data) +#define MREPEAT101(macro, data) MREPEAT100(macro, data) macro(100, data) +#define MREPEAT102(macro, data) MREPEAT101(macro, data) macro(101, data) +#define MREPEAT103(macro, data) MREPEAT102(macro, data) macro(102, data) +#define MREPEAT104(macro, data) MREPEAT103(macro, data) macro(103, data) +#define MREPEAT105(macro, data) MREPEAT104(macro, data) macro(104, data) +#define MREPEAT106(macro, data) MREPEAT105(macro, data) macro(105, data) +#define MREPEAT107(macro, data) MREPEAT106(macro, data) macro(106, data) +#define MREPEAT108(macro, data) MREPEAT107(macro, data) macro(107, data) +#define MREPEAT109(macro, data) MREPEAT108(macro, data) macro(108, data) +#define MREPEAT110(macro, data) MREPEAT109(macro, data) macro(109, data) +#define MREPEAT111(macro, data) MREPEAT110(macro, data) macro(110, data) +#define MREPEAT112(macro, data) MREPEAT111(macro, data) macro(111, data) +#define MREPEAT113(macro, data) MREPEAT112(macro, data) macro(112, data) +#define MREPEAT114(macro, data) MREPEAT113(macro, data) macro(113, data) +#define MREPEAT115(macro, data) MREPEAT114(macro, data) macro(114, data) +#define MREPEAT116(macro, data) MREPEAT115(macro, data) macro(115, data) +#define MREPEAT117(macro, data) MREPEAT116(macro, data) macro(116, data) +#define MREPEAT118(macro, data) MREPEAT117(macro, data) macro(117, data) +#define MREPEAT119(macro, data) MREPEAT118(macro, data) macro(118, data) +#define MREPEAT120(macro, data) MREPEAT119(macro, data) macro(119, data) +#define MREPEAT121(macro, data) MREPEAT120(macro, data) macro(120, data) +#define MREPEAT122(macro, data) MREPEAT121(macro, data) macro(121, data) +#define MREPEAT123(macro, data) MREPEAT122(macro, data) macro(122, data) +#define MREPEAT124(macro, data) MREPEAT123(macro, data) macro(123, data) +#define MREPEAT125(macro, data) MREPEAT124(macro, data) macro(124, data) +#define MREPEAT126(macro, data) MREPEAT125(macro, data) macro(125, data) +#define MREPEAT127(macro, data) MREPEAT126(macro, data) macro(126, data) +#define MREPEAT128(macro, data) MREPEAT127(macro, data) macro(127, data) +#define MREPEAT129(macro, data) MREPEAT128(macro, data) macro(128, data) +#define MREPEAT130(macro, data) MREPEAT129(macro, data) macro(129, data) +#define MREPEAT131(macro, data) MREPEAT130(macro, data) macro(130, data) +#define MREPEAT132(macro, data) MREPEAT131(macro, data) macro(131, data) +#define MREPEAT133(macro, data) MREPEAT132(macro, data) macro(132, data) +#define MREPEAT134(macro, data) MREPEAT133(macro, data) macro(133, data) +#define MREPEAT135(macro, data) MREPEAT134(macro, data) macro(134, data) +#define MREPEAT136(macro, data) MREPEAT135(macro, data) macro(135, data) +#define MREPEAT137(macro, data) MREPEAT136(macro, data) macro(136, data) +#define MREPEAT138(macro, data) MREPEAT137(macro, data) macro(137, data) +#define MREPEAT139(macro, data) MREPEAT138(macro, data) macro(138, data) +#define MREPEAT140(macro, data) MREPEAT139(macro, data) macro(139, data) +#define MREPEAT141(macro, data) MREPEAT140(macro, data) macro(140, data) +#define MREPEAT142(macro, data) MREPEAT141(macro, data) macro(141, data) +#define MREPEAT143(macro, data) MREPEAT142(macro, data) macro(142, data) +#define MREPEAT144(macro, data) MREPEAT143(macro, data) macro(143, data) +#define MREPEAT145(macro, data) MREPEAT144(macro, data) macro(144, data) +#define MREPEAT146(macro, data) MREPEAT145(macro, data) macro(145, data) +#define MREPEAT147(macro, data) MREPEAT146(macro, data) macro(146, data) +#define MREPEAT148(macro, data) MREPEAT147(macro, data) macro(147, data) +#define MREPEAT149(macro, data) MREPEAT148(macro, data) macro(148, data) +#define MREPEAT150(macro, data) MREPEAT149(macro, data) macro(149, data) +#define MREPEAT151(macro, data) MREPEAT150(macro, data) macro(150, data) +#define MREPEAT152(macro, data) MREPEAT151(macro, data) macro(151, data) +#define MREPEAT153(macro, data) MREPEAT152(macro, data) macro(152, data) +#define MREPEAT154(macro, data) MREPEAT153(macro, data) macro(153, data) +#define MREPEAT155(macro, data) MREPEAT154(macro, data) macro(154, data) +#define MREPEAT156(macro, data) MREPEAT155(macro, data) macro(155, data) +#define MREPEAT157(macro, data) MREPEAT156(macro, data) macro(156, data) +#define MREPEAT158(macro, data) MREPEAT157(macro, data) macro(157, data) +#define MREPEAT159(macro, data) MREPEAT158(macro, data) macro(158, data) +#define MREPEAT160(macro, data) MREPEAT159(macro, data) macro(159, data) +#define MREPEAT161(macro, data) MREPEAT160(macro, data) macro(160, data) +#define MREPEAT162(macro, data) MREPEAT161(macro, data) macro(161, data) +#define MREPEAT163(macro, data) MREPEAT162(macro, data) macro(162, data) +#define MREPEAT164(macro, data) MREPEAT163(macro, data) macro(163, data) +#define MREPEAT165(macro, data) MREPEAT164(macro, data) macro(164, data) +#define MREPEAT166(macro, data) MREPEAT165(macro, data) macro(165, data) +#define MREPEAT167(macro, data) MREPEAT166(macro, data) macro(166, data) +#define MREPEAT168(macro, data) MREPEAT167(macro, data) macro(167, data) +#define MREPEAT169(macro, data) MREPEAT168(macro, data) macro(168, data) +#define MREPEAT170(macro, data) MREPEAT169(macro, data) macro(169, data) +#define MREPEAT171(macro, data) MREPEAT170(macro, data) macro(170, data) +#define MREPEAT172(macro, data) MREPEAT171(macro, data) macro(171, data) +#define MREPEAT173(macro, data) MREPEAT172(macro, data) macro(172, data) +#define MREPEAT174(macro, data) MREPEAT173(macro, data) macro(173, data) +#define MREPEAT175(macro, data) MREPEAT174(macro, data) macro(174, data) +#define MREPEAT176(macro, data) MREPEAT175(macro, data) macro(175, data) +#define MREPEAT177(macro, data) MREPEAT176(macro, data) macro(176, data) +#define MREPEAT178(macro, data) MREPEAT177(macro, data) macro(177, data) +#define MREPEAT179(macro, data) MREPEAT178(macro, data) macro(178, data) +#define MREPEAT180(macro, data) MREPEAT179(macro, data) macro(179, data) +#define MREPEAT181(macro, data) MREPEAT180(macro, data) macro(180, data) +#define MREPEAT182(macro, data) MREPEAT181(macro, data) macro(181, data) +#define MREPEAT183(macro, data) MREPEAT182(macro, data) macro(182, data) +#define MREPEAT184(macro, data) MREPEAT183(macro, data) macro(183, data) +#define MREPEAT185(macro, data) MREPEAT184(macro, data) macro(184, data) +#define MREPEAT186(macro, data) MREPEAT185(macro, data) macro(185, data) +#define MREPEAT187(macro, data) MREPEAT186(macro, data) macro(186, data) +#define MREPEAT188(macro, data) MREPEAT187(macro, data) macro(187, data) +#define MREPEAT189(macro, data) MREPEAT188(macro, data) macro(188, data) +#define MREPEAT190(macro, data) MREPEAT189(macro, data) macro(189, data) +#define MREPEAT191(macro, data) MREPEAT190(macro, data) macro(190, data) +#define MREPEAT192(macro, data) MREPEAT191(macro, data) macro(191, data) +#define MREPEAT193(macro, data) MREPEAT192(macro, data) macro(192, data) +#define MREPEAT194(macro, data) MREPEAT193(macro, data) macro(193, data) +#define MREPEAT195(macro, data) MREPEAT194(macro, data) macro(194, data) +#define MREPEAT196(macro, data) MREPEAT195(macro, data) macro(195, data) +#define MREPEAT197(macro, data) MREPEAT196(macro, data) macro(196, data) +#define MREPEAT198(macro, data) MREPEAT197(macro, data) macro(197, data) +#define MREPEAT199(macro, data) MREPEAT198(macro, data) macro(198, data) +#define MREPEAT200(macro, data) MREPEAT199(macro, data) macro(199, data) +#define MREPEAT201(macro, data) MREPEAT200(macro, data) macro(200, data) +#define MREPEAT202(macro, data) MREPEAT201(macro, data) macro(201, data) +#define MREPEAT203(macro, data) MREPEAT202(macro, data) macro(202, data) +#define MREPEAT204(macro, data) MREPEAT203(macro, data) macro(203, data) +#define MREPEAT205(macro, data) MREPEAT204(macro, data) macro(204, data) +#define MREPEAT206(macro, data) MREPEAT205(macro, data) macro(205, data) +#define MREPEAT207(macro, data) MREPEAT206(macro, data) macro(206, data) +#define MREPEAT208(macro, data) MREPEAT207(macro, data) macro(207, data) +#define MREPEAT209(macro, data) MREPEAT208(macro, data) macro(208, data) +#define MREPEAT210(macro, data) MREPEAT209(macro, data) macro(209, data) +#define MREPEAT211(macro, data) MREPEAT210(macro, data) macro(210, data) +#define MREPEAT212(macro, data) MREPEAT211(macro, data) macro(211, data) +#define MREPEAT213(macro, data) MREPEAT212(macro, data) macro(212, data) +#define MREPEAT214(macro, data) MREPEAT213(macro, data) macro(213, data) +#define MREPEAT215(macro, data) MREPEAT214(macro, data) macro(214, data) +#define MREPEAT216(macro, data) MREPEAT215(macro, data) macro(215, data) +#define MREPEAT217(macro, data) MREPEAT216(macro, data) macro(216, data) +#define MREPEAT218(macro, data) MREPEAT217(macro, data) macro(217, data) +#define MREPEAT219(macro, data) MREPEAT218(macro, data) macro(218, data) +#define MREPEAT220(macro, data) MREPEAT219(macro, data) macro(219, data) +#define MREPEAT221(macro, data) MREPEAT220(macro, data) macro(220, data) +#define MREPEAT222(macro, data) MREPEAT221(macro, data) macro(221, data) +#define MREPEAT223(macro, data) MREPEAT222(macro, data) macro(222, data) +#define MREPEAT224(macro, data) MREPEAT223(macro, data) macro(223, data) +#define MREPEAT225(macro, data) MREPEAT224(macro, data) macro(224, data) +#define MREPEAT226(macro, data) MREPEAT225(macro, data) macro(225, data) +#define MREPEAT227(macro, data) MREPEAT226(macro, data) macro(226, data) +#define MREPEAT228(macro, data) MREPEAT227(macro, data) macro(227, data) +#define MREPEAT229(macro, data) MREPEAT228(macro, data) macro(228, data) +#define MREPEAT230(macro, data) MREPEAT229(macro, data) macro(229, data) +#define MREPEAT231(macro, data) MREPEAT230(macro, data) macro(230, data) +#define MREPEAT232(macro, data) MREPEAT231(macro, data) macro(231, data) +#define MREPEAT233(macro, data) MREPEAT232(macro, data) macro(232, data) +#define MREPEAT234(macro, data) MREPEAT233(macro, data) macro(233, data) +#define MREPEAT235(macro, data) MREPEAT234(macro, data) macro(234, data) +#define MREPEAT236(macro, data) MREPEAT235(macro, data) macro(235, data) +#define MREPEAT237(macro, data) MREPEAT236(macro, data) macro(236, data) +#define MREPEAT238(macro, data) MREPEAT237(macro, data) macro(237, data) +#define MREPEAT239(macro, data) MREPEAT238(macro, data) macro(238, data) +#define MREPEAT240(macro, data) MREPEAT239(macro, data) macro(239, data) +#define MREPEAT241(macro, data) MREPEAT240(macro, data) macro(240, data) +#define MREPEAT242(macro, data) MREPEAT241(macro, data) macro(241, data) +#define MREPEAT243(macro, data) MREPEAT242(macro, data) macro(242, data) +#define MREPEAT244(macro, data) MREPEAT243(macro, data) macro(243, data) +#define MREPEAT245(macro, data) MREPEAT244(macro, data) macro(244, data) +#define MREPEAT246(macro, data) MREPEAT245(macro, data) macro(245, data) +#define MREPEAT247(macro, data) MREPEAT246(macro, data) macro(246, data) +#define MREPEAT248(macro, data) MREPEAT247(macro, data) macro(247, data) +#define MREPEAT249(macro, data) MREPEAT248(macro, data) macro(248, data) +#define MREPEAT250(macro, data) MREPEAT249(macro, data) macro(249, data) +#define MREPEAT251(macro, data) MREPEAT250(macro, data) macro(250, data) +#define MREPEAT252(macro, data) MREPEAT251(macro, data) macro(251, data) +#define MREPEAT253(macro, data) MREPEAT252(macro, data) macro(252, data) +#define MREPEAT254(macro, data) MREPEAT253(macro, data) macro(253, data) +#define MREPEAT255(macro, data) MREPEAT254(macro, data) macro(254, data) +#define MREPEAT256(macro, data) MREPEAT255(macro, data) macro(255, data) + +/** + * \} + */ + +#endif // _MREPEAT_H_ diff --git a/src/HAL/DUE/usb/osc.h b/src/HAL/DUE/usb/osc.h new file mode 100644 index 0000000..953bcbb --- /dev/null +++ b/src/HAL/DUE/usb/osc.h @@ -0,0 +1,261 @@ +/** + * \file + * + * \brief Chip-specific oscillator management functions. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef CHIP_OSC_H_INCLUDED +#define CHIP_OSC_H_INCLUDED + +#include "compiler.h" + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +/* + * Below BOARD_XXX macros are related to the specific board, and + * should be defined by the board code, otherwise default value are used. + */ +#ifndef BOARD_FREQ_SLCK_XTAL +# warning The board slow clock xtal frequency has not been defined. +# define BOARD_FREQ_SLCK_XTAL (32768UL) +#endif + +#ifndef BOARD_FREQ_SLCK_BYPASS +# warning The board slow clock bypass frequency has not been defined. +# define BOARD_FREQ_SLCK_BYPASS (32768UL) +#endif + +#ifndef BOARD_FREQ_MAINCK_XTAL +# warning The board main clock xtal frequency has not been defined. +# define BOARD_FREQ_MAINCK_XTAL (12000000UL) +#endif + +#ifndef BOARD_FREQ_MAINCK_BYPASS +# warning The board main clock bypass frequency has not been defined. +# define BOARD_FREQ_MAINCK_BYPASS (12000000UL) +#endif + +#ifndef BOARD_OSC_STARTUP_US +# warning The board main clock xtal startup time has not been defined. +# define BOARD_OSC_STARTUP_US (15625UL) +#endif + +/** + * \weakgroup osc_group + * @{ + */ + +//! \name Oscillator identifiers +//@{ +#define OSC_SLCK_32K_RC 0 //!< Internal 32kHz RC oscillator. +#define OSC_SLCK_32K_XTAL 1 //!< External 32kHz crystal oscillator. +#define OSC_SLCK_32K_BYPASS 2 //!< External 32kHz bypass oscillator. +#define OSC_MAINCK_4M_RC 3 //!< Internal 4MHz RC oscillator. +#define OSC_MAINCK_8M_RC 4 //!< Internal 8MHz RC oscillator. +#define OSC_MAINCK_12M_RC 5 //!< Internal 12MHz RC oscillator. +#define OSC_MAINCK_XTAL 6 //!< External crystal oscillator. +#define OSC_MAINCK_BYPASS 7 //!< External bypass oscillator. +//@} + +//! \name Oscillator clock speed in hertz +//@{ +#define OSC_SLCK_32K_RC_HZ CHIP_FREQ_SLCK_RC //!< Internal 32kHz RC oscillator. +#define OSC_SLCK_32K_XTAL_HZ BOARD_FREQ_SLCK_XTAL //!< External 32kHz crystal oscillator. +#define OSC_SLCK_32K_BYPASS_HZ BOARD_FREQ_SLCK_BYPASS //!< External 32kHz bypass oscillator. +#define OSC_MAINCK_4M_RC_HZ CHIP_FREQ_MAINCK_RC_4MHZ //!< Internal 4MHz RC oscillator. +#define OSC_MAINCK_8M_RC_HZ CHIP_FREQ_MAINCK_RC_8MHZ //!< Internal 8MHz RC oscillator. +#define OSC_MAINCK_12M_RC_HZ CHIP_FREQ_MAINCK_RC_12MHZ //!< Internal 12MHz RC oscillator. +#define OSC_MAINCK_XTAL_HZ BOARD_FREQ_MAINCK_XTAL //!< External crystal oscillator. +#define OSC_MAINCK_BYPASS_HZ BOARD_FREQ_MAINCK_BYPASS //!< External bypass oscillator. +//@} + +static inline void osc_enable(uint32_t ul_id) +{ + switch (ul_id) { + case OSC_SLCK_32K_RC: + break; + + case OSC_SLCK_32K_XTAL: + pmc_switch_sclk_to_32kxtal(PMC_OSC_XTAL); + break; + + case OSC_SLCK_32K_BYPASS: + pmc_switch_sclk_to_32kxtal(PMC_OSC_BYPASS); + break; + + + case OSC_MAINCK_4M_RC: + pmc_switch_mainck_to_fastrc(CKGR_MOR_MOSCRCF_4_MHz); + break; + + case OSC_MAINCK_8M_RC: + pmc_switch_mainck_to_fastrc(CKGR_MOR_MOSCRCF_8_MHz); + break; + + case OSC_MAINCK_12M_RC: + pmc_switch_mainck_to_fastrc(CKGR_MOR_MOSCRCF_12_MHz); + break; + + + case OSC_MAINCK_XTAL: + pmc_switch_mainck_to_xtal(PMC_OSC_XTAL/*, + pmc_us_to_moscxtst(BOARD_OSC_STARTUP_US, + OSC_SLCK_32K_RC_HZ)*/); + break; + + case OSC_MAINCK_BYPASS: + pmc_switch_mainck_to_xtal(PMC_OSC_BYPASS/*, + pmc_us_to_moscxtst(BOARD_OSC_STARTUP_US, + OSC_SLCK_32K_RC_HZ)*/); + break; + } +} + +static inline void osc_disable(uint32_t ul_id) +{ + switch (ul_id) { + case OSC_SLCK_32K_RC: + case OSC_SLCK_32K_XTAL: + case OSC_SLCK_32K_BYPASS: + break; + + case OSC_MAINCK_4M_RC: + case OSC_MAINCK_8M_RC: + case OSC_MAINCK_12M_RC: + pmc_osc_disable_fastrc(); + break; + + case OSC_MAINCK_XTAL: + pmc_osc_disable_xtal(PMC_OSC_XTAL); + break; + + case OSC_MAINCK_BYPASS: + pmc_osc_disable_xtal(PMC_OSC_BYPASS); + break; + } +} + +static inline bool osc_is_ready(uint32_t ul_id) +{ + switch (ul_id) { + case OSC_SLCK_32K_RC: + return 1; + + case OSC_SLCK_32K_XTAL: + case OSC_SLCK_32K_BYPASS: + return pmc_osc_is_ready_32kxtal(); + + case OSC_MAINCK_4M_RC: + case OSC_MAINCK_8M_RC: + case OSC_MAINCK_12M_RC: + case OSC_MAINCK_XTAL: + case OSC_MAINCK_BYPASS: + return pmc_osc_is_ready_mainck(); + } + + return 0; +} + +static inline uint32_t osc_get_rate(uint32_t ul_id) +{ + switch (ul_id) { + case OSC_SLCK_32K_RC: + return OSC_SLCK_32K_RC_HZ; + + case OSC_SLCK_32K_XTAL: + return BOARD_FREQ_SLCK_XTAL; + + case OSC_SLCK_32K_BYPASS: + return BOARD_FREQ_SLCK_BYPASS; + + case OSC_MAINCK_4M_RC: + return OSC_MAINCK_4M_RC_HZ; + + case OSC_MAINCK_8M_RC: + return OSC_MAINCK_8M_RC_HZ; + + case OSC_MAINCK_12M_RC: + return OSC_MAINCK_12M_RC_HZ; + + case OSC_MAINCK_XTAL: + return BOARD_FREQ_MAINCK_XTAL; + + case OSC_MAINCK_BYPASS: + return BOARD_FREQ_MAINCK_BYPASS; + } + + return 0; +} + +/** + * \brief Wait until the oscillator identified by \a id is ready + * + * This function will busy-wait for the oscillator identified by \a id + * to become stable and ready to use as a clock source. + * + * \param id A number identifying the oscillator to wait for. + */ +static inline void osc_wait_ready(uint8_t id) +{ + while (!osc_is_ready(id)) { + /* Do nothing */ + } +} + +//! @} + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif /* CHIP_OSC_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/pll.h b/src/HAL/DUE/usb/pll.h new file mode 100644 index 0000000..8eaf276 --- /dev/null +++ b/src/HAL/DUE/usb/pll.h @@ -0,0 +1,288 @@ +/** + * \file + * + * \brief Chip-specific PLL definitions. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef CHIP_PLL_H_INCLUDED +#define CHIP_PLL_H_INCLUDED + +#include "osc.h" + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +/** + * \weakgroup pll_group + * @{ + */ + +#define PLL_OUTPUT_MIN_HZ 84000000 +#define PLL_OUTPUT_MAX_HZ 192000000 + +#define PLL_INPUT_MIN_HZ 8000000 +#define PLL_INPUT_MAX_HZ 16000000 + +#define NR_PLLS 2 +#define PLLA_ID 0 +#define UPLL_ID 1 //!< USB UTMI PLL. + +#define PLL_UPLL_HZ 480000000 + +#define PLL_COUNT 0x3FU + +enum pll_source { + PLL_SRC_MAINCK_4M_RC = OSC_MAINCK_4M_RC, //!< Internal 4MHz RC oscillator. + PLL_SRC_MAINCK_8M_RC = OSC_MAINCK_8M_RC, //!< Internal 8MHz RC oscillator. + PLL_SRC_MAINCK_12M_RC = OSC_MAINCK_12M_RC, //!< Internal 12MHz RC oscillator. + PLL_SRC_MAINCK_XTAL = OSC_MAINCK_XTAL, //!< External crystal oscillator. + PLL_SRC_MAINCK_BYPASS = OSC_MAINCK_BYPASS, //!< External bypass oscillator. + PLL_NR_SOURCES, //!< Number of PLL sources. +}; + +struct pll_config { + uint32_t ctrl; +}; + +#define pll_get_default_rate(pll_id) \ + ((osc_get_rate(CONFIG_PLL##pll_id##_SOURCE) \ + * CONFIG_PLL##pll_id##_MUL) \ + / CONFIG_PLL##pll_id##_DIV) + +/* Force UTMI PLL parameters (Hardware defined) */ +#ifdef CONFIG_PLL1_SOURCE +# undef CONFIG_PLL1_SOURCE +#endif +#ifdef CONFIG_PLL1_MUL +# undef CONFIG_PLL1_MUL +#endif +#ifdef CONFIG_PLL1_DIV +# undef CONFIG_PLL1_DIV +#endif +#define CONFIG_PLL1_SOURCE PLL_SRC_MAINCK_XTAL +#define CONFIG_PLL1_MUL 0 +#define CONFIG_PLL1_DIV 0 + +/** + * \note The SAM3X PLL hardware interprets mul as mul+1. For readability the hardware mul+1 + * is hidden in this implementation. Use mul as mul effective value. + */ +static inline void pll_config_init(struct pll_config *p_cfg, + enum pll_source e_src, uint32_t ul_div, uint32_t ul_mul) +{ + uint32_t vco_hz; + + Assert(e_src < PLL_NR_SOURCES); + + if (ul_div == 0 && ul_mul == 0) { /* Must only be true for UTMI PLL */ + p_cfg->ctrl = CKGR_UCKR_UPLLCOUNT(PLL_COUNT); + } else { /* PLLA */ + /* Calculate internal VCO frequency */ + vco_hz = osc_get_rate(e_src) / ul_div; + Assert(vco_hz >= PLL_INPUT_MIN_HZ); + Assert(vco_hz <= PLL_INPUT_MAX_HZ); + + vco_hz *= ul_mul; + Assert(vco_hz >= PLL_OUTPUT_MIN_HZ); + Assert(vco_hz <= PLL_OUTPUT_MAX_HZ); + + /* PMC hardware will automatically make it mul+1 */ + p_cfg->ctrl = CKGR_PLLAR_MULA(ul_mul - 1) | CKGR_PLLAR_DIVA(ul_div) | CKGR_PLLAR_PLLACOUNT(PLL_COUNT); + } +} + +#define pll_config_defaults(cfg, pll_id) \ + pll_config_init(cfg, \ + CONFIG_PLL##pll_id##_SOURCE, \ + CONFIG_PLL##pll_id##_DIV, \ + CONFIG_PLL##pll_id##_MUL) + +static inline void pll_config_read(struct pll_config *p_cfg, uint32_t ul_pll_id) +{ + Assert(ul_pll_id < NR_PLLS); + + if (ul_pll_id == PLLA_ID) { + p_cfg->ctrl = PMC->CKGR_PLLAR; + } else { + p_cfg->ctrl = PMC->CKGR_UCKR; + } +} + +static inline void pll_config_write(const struct pll_config *p_cfg, uint32_t ul_pll_id) +{ + Assert(ul_pll_id < NR_PLLS); + + if (ul_pll_id == PLLA_ID) { + pmc_disable_pllack(); // Always stop PLL first! + PMC->CKGR_PLLAR = CKGR_PLLAR_ONE | p_cfg->ctrl; + } else { + PMC->CKGR_UCKR = p_cfg->ctrl; + } +} + +static inline void pll_enable(const struct pll_config *p_cfg, uint32_t ul_pll_id) +{ + Assert(ul_pll_id < NR_PLLS); + + if (ul_pll_id == PLLA_ID) { + pmc_disable_pllack(); // Always stop PLL first! + PMC->CKGR_PLLAR = CKGR_PLLAR_ONE | p_cfg->ctrl; + } else { + PMC->CKGR_UCKR = p_cfg->ctrl | CKGR_UCKR_UPLLEN; + } +} + +/** + * \note This will only disable the selected PLL, not the underlying oscillator (mainck). + */ +static inline void pll_disable(uint32_t ul_pll_id) +{ + Assert(ul_pll_id < NR_PLLS); + + if (ul_pll_id == PLLA_ID) { + pmc_disable_pllack(); + } else { + PMC->CKGR_UCKR &= ~CKGR_UCKR_UPLLEN; + } +} + +static inline uint32_t pll_is_locked(uint32_t ul_pll_id) +{ + Assert(ul_pll_id < NR_PLLS); + + if (ul_pll_id == PLLA_ID) { + return pmc_is_locked_pllack(); + } else { + return pmc_is_locked_upll(); + } +} + +static inline void pll_enable_source(enum pll_source e_src) +{ + switch (e_src) { + case PLL_SRC_MAINCK_4M_RC: + case PLL_SRC_MAINCK_8M_RC: + case PLL_SRC_MAINCK_12M_RC: + case PLL_SRC_MAINCK_XTAL: + case PLL_SRC_MAINCK_BYPASS: + osc_enable(e_src); + osc_wait_ready(e_src); + break; + + default: + Assert(false); + break; + } +} + +static inline void pll_enable_config_defaults(unsigned int ul_pll_id) +{ + struct pll_config pllcfg; + + if (pll_is_locked(ul_pll_id)) { + return; // Pll already running + } + switch (ul_pll_id) { +#ifdef CONFIG_PLL0_SOURCE + case 0: + pll_enable_source(CONFIG_PLL0_SOURCE); + pll_config_init(&pllcfg, + CONFIG_PLL0_SOURCE, + CONFIG_PLL0_DIV, + CONFIG_PLL0_MUL); + break; +#endif +#ifdef CONFIG_PLL1_SOURCE + case 1: + pll_enable_source(CONFIG_PLL1_SOURCE); + pll_config_init(&pllcfg, + CONFIG_PLL1_SOURCE, + CONFIG_PLL1_DIV, + CONFIG_PLL1_MUL); + break; +#endif + default: + Assert(false); + break; + } + pll_enable(&pllcfg, ul_pll_id); + while (!pll_is_locked(ul_pll_id)); +} + +/** + * \brief Wait for PLL \a pll_id to become locked + * + * \todo Use a timeout to avoid waiting forever and hanging the system + * + * \param pll_id The ID of the PLL to wait for. + * + * \retval STATUS_OK The PLL is now locked. + * \retval ERR_TIMEOUT Timed out waiting for PLL to become locked. + */ +static inline int pll_wait_for_lock(unsigned int pll_id) +{ + Assert(pll_id < NR_PLLS); + + while (!pll_is_locked(pll_id)) { + /* Do nothing */ + } + + return 0; +} + +//! @} + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif /* CHIP_PLL_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/preprocessor.h b/src/HAL/DUE/usb/preprocessor.h new file mode 100644 index 0000000..c12d01c --- /dev/null +++ b/src/HAL/DUE/usb/preprocessor.h @@ -0,0 +1,55 @@ +/** + * \file + * + * \brief Preprocessor utils. + * + * Copyright (c) 2010-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _PREPROCESSOR_H_ +#define _PREPROCESSOR_H_ + +#include "tpaste.h" +#include "stringz.h" +#include "mrepeat.h" + + +#endif // _PREPROCESSOR_H_ diff --git a/src/HAL/DUE/usb/sbc_protocol.h b/src/HAL/DUE/usb/sbc_protocol.h new file mode 100644 index 0000000..ab84573 --- /dev/null +++ b/src/HAL/DUE/usb/sbc_protocol.h @@ -0,0 +1,173 @@ +/** + * \file + * + * \brief SCSI Block Commands + * + * This file contains definitions of some of the commands found in the + * SCSI SBC-2 standard. + * + * Note that the SBC specification depends on several commands defined + * by the SCSI Primary Commands (SPC) standard. Each version of the SBC + * standard is meant to be used in conjunction with a specific version + * of the SPC standard, as follows: + * - SBC depends on SPC + * - SBC-2 depends on SPC-3 + * - SBC-3 depends on SPC-4 + * + * Copyright (c) 2014-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ +#ifndef _SBC_PROTOCOL_H_ +#define _SBC_PROTOCOL_H_ + + +/** + * \ingroup usb_msc_protocol + * \defgroup usb_sbc_protocol SCSI Block Commands protocol definitions + * + * @{ + */ + +//! \name SCSI commands defined by SBC-2 +//@{ +#define SBC_FORMAT_UNIT 0x04 +#define SBC_READ6 0x08 +#define SBC_WRITE6 0x0A +#define SBC_START_STOP_UNIT 0x1B +#define SBC_READ_CAPACITY10 0x25 +#define SBC_READ10 0x28 +#define SBC_WRITE10 0x2A +#define SBC_VERIFY10 0x2F +//@} + +//! \name SBC-2 Mode page definitions +//@{ + +enum scsi_sbc_mode { + SCSI_MS_MODE_RW_ERR_RECOV = 0x01, //!< Read-Write Error Recovery mode page + SCSI_MS_MODE_FORMAT_DEVICE = 0x03, //!< Format Device mode page + SCSI_MS_MODE_FLEXIBLE_DISK = 0x05, //!< Flexible Disk mode page + SCSI_MS_MODE_CACHING = 0x08, //!< Caching mode page +}; + + +//! \name SBC-2 Device-Specific Parameter +//@{ +#define SCSI_MS_SBC_WP 0x80 //!< Write Protected +#define SCSI_MS_SBC_DPOFUA 0x10 //!< DPO and FUA supported +//@} + +/** + * \brief SBC-2 Short LBA mode parameter block descriptor + */ +struct sbc_slba_block_desc { + be32_t nr_blocks; //!< Number of Blocks + be32_t block_len; //!< Block Length +#define SBC_SLBA_BLOCK_LEN_MASK 0x00FFFFFFU //!< Mask reserved bits +}; + +/** + * \brief SBC-2 Caching mode page + */ +struct sbc_caching_mode_page { + uint8_t page_code; + uint8_t page_length; + uint8_t flags2; +#define SBC_MP_CACHE_IC (1 << 7) //!< Initiator Control +#define SBC_MP_CACHE_ABPF (1 << 6) //!< Abort Pre-Fetch +#define SBC_MP_CACHE_CAP (1 << 5) //!< Catching Analysis Permitted +#define SBC_MP_CACHE_DISC (1 << 4) //!< Discontinuity +#define SBC_MP_CACHE_SIZE (1 << 3) //!< Size enable +#define SBC_MP_CACHE_WCE (1 << 2) //!< Write back Cache Enable +#define SBC_MP_CACHE_MF (1 << 1) //!< Multiplication Factor +#define SBC_MP_CACHE_RCD (1 << 0) //!< Read Cache Disable + uint8_t retention; + be16_t dis_pf_transfer_len; + be16_t min_prefetch; + be16_t max_prefetch; + be16_t max_prefetch_ceil; + uint8_t flags12; +#define SBC_MP_CACHE_FSW (1 << 7) //!< Force Sequential Write +#define SBC_MP_CACHE_LBCSS (1 << 6) //!< Logical Blk Cache Seg Sz +#define SBC_MP_CACHE_DRA (1 << 5) //!< Disable Read-Ahead +#define SBC_MP_CACHE_NV_DIS (1 << 0) //!< Non-Volatile Cache Disable + uint8_t nr_cache_segments; + be16_t cache_segment_size; + uint8_t reserved[4]; +}; + +/** + * \brief SBC-2 Read-Write Error Recovery mode page + */ +struct sbc_rdwr_error_recovery_mode_page { + uint8_t page_code; + uint8_t page_length; +#define SPC_MP_RW_ERR_RECOV_PAGE_LENGTH 0x0A + uint8_t flags1; +#define SBC_MP_RW_ERR_RECOV_AWRE (1 << 7) +#define SBC_MP_RW_ERR_RECOV_ARRE (1 << 6) +#define SBC_MP_RW_ERR_RECOV_TB (1 << 5) +#define SBC_MP_RW_ERR_RECOV_RC (1 << 4) +#define SBC_MP_RW_ERR_RECOV_ERR (1 << 3) +#define SBC_MP_RW_ERR_RECOV_PER (1 << 2) +#define SBC_MP_RW_ERR_RECOV_DTE (1 << 1) +#define SBC_MP_RW_ERR_RECOV_DCR (1 << 0) + uint8_t read_retry_count; + uint8_t correction_span; + uint8_t head_offset_count; + uint8_t data_strobe_offset_count; + uint8_t flags2; + uint8_t write_retry_count; + uint8_t flags3; + be16_t recovery_time_limit; +}; +//@} + +/** + * \brief SBC-2 READ CAPACITY (10) parameter data + */ +struct sbc_read_capacity10_data { + be32_t max_lba; //!< LBA of last logical block + be32_t block_len; //!< Number of bytes in the last logical block +}; + +//@} + +#endif // _SBC_PROTOCOL_H_ diff --git a/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp b/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp new file mode 100644 index 0000000..34cc256 --- /dev/null +++ b/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp @@ -0,0 +1,142 @@ +/** + * Interface from Atmel USB MSD to Marlin SD card + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(SDSUPPORT) + +#include "../../../sd/cardreader.h" +extern "C" { + #include "sd_mmc_spi_mem.h" +} + +#define SD_MMC_BLOCK_SIZE 512 + +void sd_mmc_spi_mem_init() { +} + +Ctrl_status sd_mmc_spi_test_unit_ready() { + #ifdef DISABLE_DUE_SD_MMC + return CTRL_NO_PRESENT; + #endif + if (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()) + return CTRL_NO_PRESENT; + return CTRL_GOOD; +} + +// NOTE: This function is defined as returning the address of the last block +// in the card, which is cardSize() - 1 +Ctrl_status sd_mmc_spi_read_capacity(uint32_t *nb_sector) { + if (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()) + return CTRL_NO_PRESENT; + *nb_sector = card.diskIODriver()->cardSize() - 1; + return CTRL_GOOD; +} + +bool sd_mmc_spi_unload(bool) { return true; } + +bool sd_mmc_spi_wr_protect() { return false; } + +bool sd_mmc_spi_removal() { + return (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()); +} + +#if ACCESS_USB == true +/** + * \name MEM <-> USB Interface + * @{ + */ + +#include "udi_msc.h" + +COMPILER_WORD_ALIGNED +uint8_t sector_buf[SD_MMC_BLOCK_SIZE]; + +// #define DEBUG_MMC + +Ctrl_status sd_mmc_spi_usb_read_10(uint32_t addr, uint16_t nb_sector) { + #ifdef DISABLE_DUE_SD_MMC + return CTRL_NO_PRESENT; + #endif + if (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()) + return CTRL_NO_PRESENT; + + #ifdef DEBUG_MMC + { + char buffer[80]; + sprintf_P(buffer, PSTR("SDRD: %d @ 0x%08x\n"), nb_sector, addr); + PORT_REDIRECT(SERIAL_PORTMASK(0)); + SERIAL_ECHO(buffer); + } + #endif + + // Start reading + if (!card.diskIODriver()->readStart(addr)) + return CTRL_FAIL; + + // For each specified sector + while (nb_sector--) { + + // Read a sector + card.diskIODriver()->readData(sector_buf); + + // RAM -> USB + if (!udi_msc_trans_block(true, sector_buf, SD_MMC_BLOCK_SIZE, nullptr)) { + card.diskIODriver()->readStop(); + return CTRL_FAIL; + } + } + + // Stop reading + card.diskIODriver()->readStop(); + + // Done + return CTRL_GOOD; +} + +Ctrl_status sd_mmc_spi_usb_write_10(uint32_t addr, uint16_t nb_sector) { + #ifdef DISABLE_DUE_SD_MMC + return CTRL_NO_PRESENT; + #endif + if (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()) + return CTRL_NO_PRESENT; + + #ifdef DEBUG_MMC + { + char buffer[80]; + sprintf_P(buffer, PSTR("SDWR: %d @ 0x%08x\n"), nb_sector, addr); + PORT_REDIRECT(SERIAL_PORTMASK(0)); + SERIAL_ECHO(buffer); + } + #endif + + if (!card.diskIODriver()->writeStart(addr, nb_sector)) + return CTRL_FAIL; + + // For each specified sector + while (nb_sector--) { + + // USB -> RAM + if (!udi_msc_trans_block(false, sector_buf, SD_MMC_BLOCK_SIZE, nullptr)) { + card.diskIODriver()->writeStop(); + return CTRL_FAIL; + } + + // Write a sector + card.diskIODriver()->writeData(sector_buf); + } + + // Stop writing + card.diskIODriver()->writeStop(); + + // Done + return CTRL_GOOD; +} + +#endif // ACCESS_USB == true + +#endif // SDSUPPORT +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/sd_mmc_spi_mem.h b/src/HAL/DUE/usb/sd_mmc_spi_mem.h new file mode 100644 index 0000000..553fd3c --- /dev/null +++ b/src/HAL/DUE/usb/sd_mmc_spi_mem.h @@ -0,0 +1,177 @@ +/***************************************************************************** + * + * \file + * + * \brief CTRL_ACCESS interface for SD/MMC card. + * + * Copyright (c) 2014-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + ******************************************************************************/ +/* + * Support and FAQ: visit Atmel Support + */ + + +#ifndef _SD_MMC_SPI_MEM_H_ +#define _SD_MMC_SPI_MEM_H_ + +/** + * \defgroup group_avr32_components_memory_sd_mmc_sd_mmc_spi_mem SD/MMC SPI Memory + * + * \ingroup group_avr32_components_memory_sd_mmc_sd_mmc_spi + * + * \{ + */ + +#include "conf_access.h" + +#if SD_MMC_SPI_MEM == DISABLE + #error sd_mmc_spi_mem.h is #included although SD_MMC_SPI_MEM is disabled +#endif + + +#include "ctrl_access.h" + + +//_____ D E F I N I T I O N S ______________________________________________ + +#define SD_MMC_REMOVED 0 +#define SD_MMC_INSERTED 1 +#define SD_MMC_REMOVING 2 + + +//---- CONTROL FUNCTIONS ---- +//! +//! @brief This function initializes the hw/sw resources required to drive the SD_MMC_SPI. +//!/ +extern void sd_mmc_spi_mem_init(void); + +//! +//! @brief This function tests the state of the SD_MMC memory and sends it to the Host. +//! For a PC, this device is seen as a removable media +//! Before indicating any modification of the status of the media (GOOD->NO_PRESENT or vice-versa), +//! the function must return the BUSY data to make the PC accepting the change +//! +//! @return Ctrl_status +//! Media is ready -> CTRL_GOOD +//! Media not present -> CTRL_NO_PRESENT +//! Media has changed -> CTRL_BUSY +//!/ +extern Ctrl_status sd_mmc_spi_test_unit_ready(void); + +//! +//! @brief This function gives the address of the last valid sector. +//! +//! @param *nb_sector number of sector (sector = 512B). OUT +//! +//! @return Ctrl_status +//! Media ready -> CTRL_GOOD +//! Media not present -> CTRL_NO_PRESENT +//!/ +extern Ctrl_status sd_mmc_spi_read_capacity(uint32_t *nb_sector); + +/*! \brief Unload/Load the SD/MMC card selected + * + * The START STOP UNIT SCSI optional command allows an application client to + * eject the removable medium on a LUN. + * + * \param unload \c true to unload the medium, \c false to load the medium. + * + * \return \c true if unload/load done success. + */ +extern bool sd_mmc_spi_unload(bool unload); + +//! +//! @brief This function returns the write protected status of the memory. +//! +//! Only used by memory removal with a HARDWARE SPECIFIC write protected detection +//! ! The user must unplug the memory to change this write protected status, +//! which cannot be for a SD_MMC. +//! +//! @return false -> the memory is not write-protected (always) +//!/ +extern bool sd_mmc_spi_wr_protect(void); + +//! +//! @brief This function tells if the memory has been removed or not. +//! +//! @return false -> The memory isn't removed +//! +extern bool sd_mmc_spi_removal(void); + + +//---- ACCESS DATA FUNCTIONS ---- + +#if ACCESS_USB == true +// Standard functions for open in read/write mode the device + +//! +//! @brief This function performs a read operation of n sectors from a given address on. +//! (sector = 512B) +//! +//! DATA FLOW is: SD_MMC => USB +//! +//! @param addr Sector address to start the read from +//! @param nb_sector Number of sectors to transfer +//! +//! @return Ctrl_status +//! It is ready -> CTRL_GOOD +//! A error occur -> CTRL_FAIL +//! +extern Ctrl_status sd_mmc_spi_usb_read_10(uint32_t addr, uint16_t nb_sector); + +//! This function initializes the SD/MMC memory for a write operation +//! +//! DATA FLOW is: USB => SD_MMC +//! +//! (sector = 512B) +//! @param addr Sector address to start write +//! @param nb_sector Number of sectors to transfer +//! +//! @return Ctrl_status +//! It is ready -> CTRL_GOOD +//! An error occurs -> CTRL_FAIL +//! +extern Ctrl_status sd_mmc_spi_usb_write_10(uint32_t addr, uint16_t nb_sector); + +#endif // #if ACCESS_USB == true + +/** + * \} + */ + +#endif // _SD_MMC_SPI_MEM_H_ diff --git a/src/HAL/DUE/usb/spc_protocol.h b/src/HAL/DUE/usb/spc_protocol.h new file mode 100644 index 0000000..d67cc5c --- /dev/null +++ b/src/HAL/DUE/usb/spc_protocol.h @@ -0,0 +1,337 @@ +/** + * \file + * + * \brief SCSI Primary Commands + * + * This file contains definitions of some of the commands found in the + * SPC-2 standard. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ + +/* + * Support and FAQ: visit Atmel Support + */ +#ifndef _SPC_PROTOCOL_H_ +#define _SPC_PROTOCOL_H_ + +/** + * \ingroup usb_msc_protocol + * \defgroup usb_spc_protocol SCSI Primary Commands protocol definitions + * + * @{ + */ + +//! \name SCSI commands defined by SPC-2 +//@{ +#define SPC_TEST_UNIT_READY 0x00 +#define SPC_REQUEST_SENSE 0x03 +#define SPC_INQUIRY 0x12 +#define SPC_MODE_SELECT6 0x15 +#define SPC_MODE_SENSE6 0x1A +#define SPC_SEND_DIAGNOSTIC 0x1D +#define SPC_PREVENT_ALLOW_MEDIUM_REMOVAL 0x1E +#define SPC_MODE_SENSE10 0x5A +#define SPC_REPORT_LUNS 0xA0 +//@} + +//! \brief May be set in byte 0 of the INQUIRY CDB +//@{ +//! Enable Vital Product Data +#define SCSI_INQ_REQ_EVPD 0x01 +//! Command Support Data specified by the PAGE OR OPERATION CODE field +#define SCSI_INQ_REQ_CMDT 0x02 +//@} + +COMPILER_PACK_SET(1) + +/** + * \brief SCSI Standard Inquiry data structure + */ +struct scsi_inquiry_data { + uint8_t pq_pdt; //!< Peripheral Qual / Peripheral Dev Type +#define SCSI_INQ_PQ_CONNECTED 0x00 //!< Peripheral connected +#define SCSI_INQ_PQ_NOT_CONN 0x20 //!< Peripheral not connected +#define SCSI_INQ_PQ_NOT_SUPP 0x60 //!< Peripheral not supported +#define SCSI_INQ_DT_DIR_ACCESS 0x00 //!< Direct Access (SBC) +#define SCSI_INQ_DT_SEQ_ACCESS 0x01 //!< Sequential Access +#define SCSI_INQ_DT_PRINTER 0x02 //!< Printer +#define SCSI_INQ_DT_PROCESSOR 0x03 //!< Processor device +#define SCSI_INQ_DT_WRITE_ONCE 0x04 //!< Write-once device +#define SCSI_INQ_DT_CD_DVD 0x05 //!< CD/DVD device +#define SCSI_INQ_DT_OPTICAL 0x07 //!< Optical Memory +#define SCSI_INQ_DT_MC 0x08 //!< Medium Changer +#define SCSI_INQ_DT_ARRAY 0x0C //!< Storage Array Controller +#define SCSI_INQ_DT_ENCLOSURE 0x0D //!< Enclosure Services +#define SCSI_INQ_DT_RBC 0x0E //!< Simplified Direct Access +#define SCSI_INQ_DT_OCRW 0x0F //!< Optical card reader/writer +#define SCSI_INQ_DT_BCC 0x10 //!< Bridge Controller Commands +#define SCSI_INQ_DT_OSD 0x11 //!< Object-based Storage +#define SCSI_INQ_DT_NONE 0x1F //!< No Peripheral + uint8_t flags1; //!< Flags (byte 1) +#define SCSI_INQ_RMB 0x80 //!< Removable Medium + uint8_t version; //!< Version +#define SCSI_INQ_VER_NONE 0x00 //!< No standards conformance +#define SCSI_INQ_VER_SPC 0x03 //!< SCSI Primary Commands (link to SBC) +#define SCSI_INQ_VER_SPC2 0x04 //!< SCSI Primary Commands - 2 (link to SBC-2) +#define SCSI_INQ_VER_SPC3 0x05 //!< SCSI Primary Commands - 3 (link to SBC-2) +#define SCSI_INQ_VER_SPC4 0x06 //!< SCSI Primary Commands - 4 (link to SBC-3) + uint8_t flags3; //!< Flags (byte 3) +#define SCSI_INQ_NORMACA 0x20 //!< Normal ACA Supported +#define SCSI_INQ_HISUP 0x10 //!< Hierarchal LUN addressing +#define SCSI_INQ_RSP_SPC2 0x02 //!< SPC-2 / SPC-3 response format + uint8_t addl_len; //!< Additional Length (n-4) +#define SCSI_INQ_ADDL_LEN(tot) ((tot)-5) //!< Total length is \a tot + uint8_t flags5; //!< Flags (byte 5) +#define SCSI_INQ_SCCS 0x80 + uint8_t flags6; //!< Flags (byte 6) +#define SCSI_INQ_BQUE 0x80 +#define SCSI_INQ_ENCSERV 0x40 +#define SCSI_INQ_MULTIP 0x10 +#define SCSI_INQ_MCHGR 0x08 +#define SCSI_INQ_ADDR16 0x01 + uint8_t flags7; //!< Flags (byte 7) +#define SCSI_INQ_WBUS16 0x20 +#define SCSI_INQ_SYNC 0x10 +#define SCSI_INQ_LINKED 0x08 +#define SCSI_INQ_CMDQUE 0x02 + uint8_t vendor_id[8]; //!< T10 Vendor Identification + uint8_t product_id[16]; //!< Product Identification + uint8_t product_rev[4]; //!< Product Revision Level +}; + +/** + * \brief SCSI Standard Request sense data structure + */ +struct scsi_request_sense_data { + /* 1st byte: REQUEST SENSE response flags*/ + uint8_t valid_reponse_code; +#define SCSI_SENSE_VALID 0x80 //!< Indicates the INFORMATION field contains valid information +#define SCSI_SENSE_RESPONSE_CODE_MASK 0x7F +#define SCSI_SENSE_CURRENT 0x70 //!< Response code 70h (current errors) +#define SCSI_SENSE_DEFERRED 0x71 + + /* 2nd byte */ + uint8_t obsolete; + + /* 3rd byte */ + uint8_t sense_flag_key; +#define SCSI_SENSE_FILEMARK 0x80 //!< Indicates that the current command has read a filemark or setmark. +#define SCSI_SENSE_EOM 0x40 //!< Indicates that an end-of-medium condition exists. +#define SCSI_SENSE_ILI 0x20 //!< Indicates that the requested logical block length did not match the logical block length of the data on the medium. +#define SCSI_SENSE_RESERVED 0x10 //!< Reserved +#define SCSI_SENSE_KEY(x) (x&0x0F) //!< Sense Key + + /* 4th to 7th bytes - INFORMATION field */ + uint8_t information[4]; + + /* 8th byte - ADDITIONAL SENSE LENGTH field */ + uint8_t AddSenseLen; +#define SCSI_SENSE_ADDL_LEN(total_len) ((total_len) - 8) + + /* 9th to 12th byte - COMMAND-SPECIFIC INFORMATION field */ + uint8_t CmdSpecINFO[4]; + + /* 13th byte - ADDITIONAL SENSE CODE field */ + uint8_t AddSenseCode; + + /* 14th byte - ADDITIONAL SENSE CODE QUALIFIER field */ + uint8_t AddSnsCodeQlfr; + + /* 15th byte - FIELD REPLACEABLE UNIT CODE field */ + uint8_t FldReplUnitCode; + + /* 16th byte */ + uint8_t SenseKeySpec[3]; +#define SCSI_SENSE_SKSV 0x80 //!< Indicates the SENSE-KEY SPECIFIC field contains valid information +}; + +COMPILER_PACK_RESET() + +/* Vital Product Data page codes */ +enum scsi_vpd_page_code { + SCSI_VPD_SUPPORTED_PAGES = 0x00, + SCSI_VPD_UNIT_SERIAL_NUMBER = 0x80, + SCSI_VPD_DEVICE_IDENTIFICATION = 0x83, +}; +#define SCSI_VPD_HEADER_SIZE 4 + +/* Constants associated with the Device Identification VPD page */ +#define SCSI_VPD_ID_HEADER_SIZE 4 + +#define SCSI_VPD_CODE_SET_BINARY 1 +#define SCSI_VPD_CODE_SET_ASCII 2 +#define SCSI_VPD_CODE_SET_UTF8 3 + +#define SCSI_VPD_ID_TYPE_T10 1 + + +/* Sense keys */ +enum scsi_sense_key { + SCSI_SK_NO_SENSE = 0x0, + SCSI_SK_RECOVERED_ERROR = 0x1, + SCSI_SK_NOT_READY = 0x2, + SCSI_SK_MEDIUM_ERROR = 0x3, + SCSI_SK_HARDWARE_ERROR = 0x4, + SCSI_SK_ILLEGAL_REQUEST = 0x5, + SCSI_SK_UNIT_ATTENTION = 0x6, + SCSI_SK_DATA_PROTECT = 0x7, + SCSI_SK_BLANK_CHECK = 0x8, + SCSI_SK_VENDOR_SPECIFIC = 0x9, + SCSI_SK_COPY_ABORTED = 0xA, + SCSI_SK_ABORTED_COMMAND = 0xB, + SCSI_SK_VOLUME_OVERFLOW = 0xD, + SCSI_SK_MISCOMPARE = 0xE, +}; + +/* Additional Sense Code / Additional Sense Code Qualifier pairs */ +enum scsi_asc_ascq { + SCSI_ASC_NO_ADDITIONAL_SENSE_INFO = 0x0000, + SCSI_ASC_LU_NOT_READY_REBUILD_IN_PROGRESS = 0x0405, + SCSI_ASC_WRITE_ERROR = 0x0C00, + SCSI_ASC_UNRECOVERED_READ_ERROR = 0x1100, + SCSI_ASC_INVALID_COMMAND_OPERATION_CODE = 0x2000, + SCSI_ASC_INVALID_FIELD_IN_CDB = 0x2400, + SCSI_ASC_WRITE_PROTECTED = 0x2700, + SCSI_ASC_NOT_READY_TO_READY_CHANGE = 0x2800, + SCSI_ASC_MEDIUM_NOT_PRESENT = 0x3A00, + SCSI_ASC_INTERNAL_TARGET_FAILURE = 0x4400, +}; + +/** + * \brief SPC-2 Mode parameter + * This subclause describes the block descriptors and the pages + * used with MODE SELECT and MODE SENSE commands + * that are applicable to all SCSI devices. + */ +enum scsi_spc_mode { + SCSI_MS_MODE_VENDOR_SPEC = 0x00, + SCSI_MS_MODE_INFEXP = 0x1C, // Informational exceptions control page + SCSI_MS_MODE_ALL = 0x3F, +}; + +/** + * \brief SPC-2 Informational exceptions control page + * See chapter 8.3.8 + */ +struct spc_control_page_info_execpt { + uint8_t page_code; + uint8_t page_length; +#define SPC_MP_INFEXP_PAGE_LENGTH 0x0A + uint8_t flags1; +#define SPC_MP_INFEXP_PERF (1<<7) //!< Initiator Control +#define SPC_MP_INFEXP_EBF (1<<5) //!< Caching Analysis Permitted +#define SPC_MP_INFEXP_EWASC (1<<4) //!< Discontinuity +#define SPC_MP_INFEXP_DEXCPT (1<<3) //!< Size enable +#define SPC_MP_INFEXP_TEST (1<<2) //!< Writeback Cache Enable +#define SPC_MP_INFEXP_LOGERR (1<<0) //!< Log errors bit + uint8_t mrie; +#define SPC_MP_INFEXP_MRIE_NO_REPORT 0x00 +#define SPC_MP_INFEXP_MRIE_ASYNC_EVENT 0x01 +#define SPC_MP_INFEXP_MRIE_GEN_UNIT 0x02 +#define SPC_MP_INFEXP_MRIE_COND_RECOV_ERROR 0x03 +#define SPC_MP_INFEXP_MRIE_UNCOND_RECOV_ERROR 0x04 +#define SPC_MP_INFEXP_MRIE_NO_SENSE 0x05 +#define SPC_MP_INFEXP_MRIE_ONLY_REPORT 0x06 + be32_t interval_timer; + be32_t report_count; +}; + + +enum scsi_spc_mode_sense_pc { + SCSI_MS_SENSE_PC_CURRENT = 0, + SCSI_MS_SENSE_PC_CHANGEABLE = 1, + SCSI_MS_SENSE_PC_DEFAULT = 2, + SCSI_MS_SENSE_PC_SAVED = 3, +}; + + + +static inline bool scsi_mode_sense_dbd_is_set(const uint8_t * cdb) +{ + return (cdb[1] >> 3) & 1; +} + +static inline uint8_t scsi_mode_sense_get_page_code(const uint8_t * cdb) +{ + return cdb[2] & 0x3F; +} + +static inline uint8_t scsi_mode_sense_get_pc(const uint8_t * cdb) +{ + return cdb[2] >> 6; +} + +/** + * \brief SCSI Mode Parameter Header used by MODE SELECT(6) and MODE + * SENSE(6) + */ +struct scsi_mode_param_header6 { + uint8_t mode_data_length; //!< Number of bytes after this + uint8_t medium_type; //!< Medium Type + uint8_t device_specific_parameter; //!< Defined by command set + uint8_t block_descriptor_length; //!< Length of block descriptors +}; + +/** + * \brief SCSI Mode Parameter Header used by MODE SELECT(10) and MODE + * SENSE(10) + */ +struct scsi_mode_param_header10 { + be16_t mode_data_length; //!< Number of bytes after this + uint8_t medium_type; //!< Medium Type + uint8_t device_specific_parameter; //!< Defined by command set + uint8_t flags4; //!< LONGLBA in bit 0 + uint8_t reserved; + be16_t block_descriptor_length; //!< Length of block descriptors +}; + +/** + * \brief SCSI Page_0 Mode Page header (SPF not set) + */ +struct scsi_mode_page_0_header { + uint8_t page_code; +#define SCSI_PAGE_CODE_PS (1 << 7) //!< Parameters Saveable +#define SCSI_PAGE_CODE_SPF (1 << 6) //!< SubPage Format + uint8_t page_length; //!< Number of bytes after this +#define SCSI_MS_PAGE_LEN(total) ((total) - 2) +}; + +//@} + +#endif // SPC_PROTOCOL_H_ diff --git a/src/HAL/DUE/usb/stringz.h b/src/HAL/DUE/usb/stringz.h new file mode 100644 index 0000000..fc9aaf3 --- /dev/null +++ b/src/HAL/DUE/usb/stringz.h @@ -0,0 +1,85 @@ +/** + * \file + * + * \brief Preprocessor stringizing utils. + * + * Copyright (c) 2010-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _STRINGZ_H_ +#define _STRINGZ_H_ + +/** + * \defgroup group_sam_utils_stringz Preprocessor - Stringize + * + * \ingroup group_sam_utils + * + * \{ + */ + +/*! \brief Stringize. + * + * Stringize a preprocessing token, this token being allowed to be \#defined. + * + * May be used only within macros with the token passed as an argument if the token is \#defined. + * + * For example, writing STRINGZ(PIN) within a macro \#defined by PIN_NAME(PIN) + * and invoked as PIN_NAME(PIN0) with PIN0 \#defined as A0 is equivalent to + * writing "A0". + */ +#define STRINGZ(x) #x + +/*! \brief Absolute stringize. + * + * Stringize a preprocessing token, this token being allowed to be \#defined. + * + * No restriction of use if the token is \#defined. + * + * For example, writing ASTRINGZ(PIN0) anywhere with PIN0 \#defined as A0 is + * equivalent to writing "A0". + */ +#define ASTRINGZ(x) STRINGZ(x) + +/** + * \} + */ + +#endif // _STRINGZ_H_ diff --git a/src/HAL/DUE/usb/sysclk.c b/src/HAL/DUE/usb/sysclk.c new file mode 100644 index 0000000..cbb4e2c --- /dev/null +++ b/src/HAL/DUE/usb/sysclk.c @@ -0,0 +1,122 @@ +/** + * \file + * + * \brief Chip-specific system clock management functions. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "sysclk.h" + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +/** + * \weakgroup sysclk_group + * @{ + */ + +#if defined(CONFIG_USBCLK_SOURCE) || defined(__DOXYGEN__) +/** + * \brief Enable full speed USB clock. + * + * \note The SAM3X PMC hardware interprets div as div+1. For readability the hardware div+1 + * is hidden in this implementation. Use div as div effective value. + * + * \param pll_id Source of the USB clock. + * \param div Actual clock divisor. Must be superior to 0. + */ +void sysclk_enable_usb(void) +{ + Assert(CONFIG_USBCLK_DIV > 0); + +#ifdef CONFIG_PLL0_SOURCE + if (CONFIG_USBCLK_SOURCE == USBCLK_SRC_PLL0) { + struct pll_config pllcfg; + + pll_enable_source(CONFIG_PLL0_SOURCE); + pll_config_defaults(&pllcfg, 0); + pll_enable(&pllcfg, 0); + pll_wait_for_lock(0); + pmc_switch_udpck_to_pllack(CONFIG_USBCLK_DIV - 1); + pmc_enable_udpck(); + return; + } +#endif + + if (CONFIG_USBCLK_SOURCE == USBCLK_SRC_UPLL) { + + pmc_enable_upll_clock(); + pmc_switch_udpck_to_upllck(CONFIG_USBCLK_DIV - 1); + pmc_enable_udpck(); + return; + } +} + +/** + * \brief Disable full speed USB clock. + * + * \note This implementation does not switch off the PLL, it just turns off the USB clock. + */ +void sysclk_disable_usb(void) +{ + pmc_disable_udpck(); +} +#endif // CONFIG_USBCLK_SOURCE + +//! @} + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/sysclk.h b/src/HAL/DUE/usb/sysclk.h new file mode 100644 index 0000000..16db8c8 --- /dev/null +++ b/src/HAL/DUE/usb/sysclk.h @@ -0,0 +1,229 @@ +/** + * \file + * + * \brief Chip-specific system clock management functions. + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef CHIP_SYSCLK_H_INCLUDED +#define CHIP_SYSCLK_H_INCLUDED + +#include "osc.h" +#include "pll.h" + +/** + * \page sysclk_quickstart Quick Start Guide for the System Clock Management service (SAM3A) + * + * This is the quick start guide for the \ref sysclk_group "System Clock Management" + * service, with step-by-step instructions on how to configure and use the service for + * specific use cases. + * + * \section sysclk_quickstart_usecases System Clock Management use cases + * - \ref sysclk_quickstart_basic + * + * \section sysclk_quickstart_basic Basic usage of the System Clock Management service + * This section will present a basic use case for the System Clock Management service. + * This use case will configure the main system clock to 84MHz, using an internal PLL + * module to multiply the frequency of a crystal attached to the microcontroller. + * + * \subsection sysclk_quickstart_use_case_1_prereq Prerequisites + * - None + * + * \subsection sysclk_quickstart_use_case_1_setup_steps Initialization code + * Add to the application initialization code: + * \code + sysclk_init(); +\endcode + * + * \subsection sysclk_quickstart_use_case_1_setup_steps_workflow Workflow + * -# Configure the system clocks according to the settings in conf_clock.h: + * \code sysclk_init(); \endcode + * + * \subsection sysclk_quickstart_use_case_1_example_code Example code + * Add or uncomment the following in your conf_clock.h header file, commenting out all other + * definitions of the same symbol(s): + * \code + #define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_PLLACK + + // Fpll0 = (Fclk * PLL_mul) / PLL_div + #define CONFIG_PLL0_SOURCE PLL_SRC_MAINCK_XTAL + #define CONFIG_PLL0_MUL (84000000UL / BOARD_FREQ_MAINCK_XTAL) + #define CONFIG_PLL0_DIV 1 + + // Fbus = Fsys / BUS_div + #define CONFIG_SYSCLK_PRES SYSCLK_PRES_1 +\endcode + * + * \subsection sysclk_quickstart_use_case_1_example_workflow Workflow + * -# Configure the main system clock to use the output of the PLL module as its source: + * \code #define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_PLLACK \endcode + * -# Configure the PLL module to use the fast external fast crystal oscillator as its source: + * \code #define CONFIG_PLL0_SOURCE PLL_SRC_MAINCK_XTAL \endcode + * -# Configure the PLL module to multiply the external fast crystal oscillator frequency up to 84MHz: + * \code + #define CONFIG_PLL0_MUL (84000000UL / BOARD_FREQ_MAINCK_XTAL) + #define CONFIG_PLL0_DIV 1 +\endcode + * \note For user boards, \c BOARD_FREQ_MAINCK_XTAL should be defined in the board \c conf_board.h configuration + * file as the frequency of the fast crystal attached to the microcontroller. + * -# Configure the main clock to run at the full 84MHz, disable scaling of the main system clock speed: + * \code + #define CONFIG_SYSCLK_PRES SYSCLK_PRES_1 +\endcode + * \note Some dividers are powers of two, while others are integer division factors. Refer to the + * formulas in the conf_clock.h template commented above each division define. + */ + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +/** + * \weakgroup sysclk_group + * @{ + */ + +//! \name Configuration Symbols +//@{ +/** + * \def CONFIG_SYSCLK_SOURCE + * \brief Initial/static main system clock source + * + * The main system clock will be configured to use this clock during + * initialization. + */ +#ifndef CONFIG_SYSCLK_SOURCE +# define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_MAINCK_4M_RC +#endif +/** + * \def CONFIG_SYSCLK_PRES + * \brief Initial CPU clock divider (mck) + * + * The MCK will run at + * \f[ + * f_{MCK} = \frac{f_{sys}}{\mathrm{CONFIG\_SYSCLK\_PRES}}\,\mbox{Hz} + * \f] + * after initialization. + */ +#ifndef CONFIG_SYSCLK_PRES +# define CONFIG_SYSCLK_PRES 0 +#endif + +//@} + +//! \name Master Clock Sources (MCK) +//@{ +#define SYSCLK_SRC_SLCK_RC 0 //!< Internal 32kHz RC oscillator as master source clock +#define SYSCLK_SRC_SLCK_XTAL 1 //!< External 32kHz crystal oscillator as master source clock +#define SYSCLK_SRC_SLCK_BYPASS 2 //!< External 32kHz bypass oscillator as master source clock +#define SYSCLK_SRC_MAINCK_4M_RC 3 //!< Internal 4MHz RC oscillator as master source clock +#define SYSCLK_SRC_MAINCK_8M_RC 4 //!< Internal 8MHz RC oscillator as master source clock +#define SYSCLK_SRC_MAINCK_12M_RC 5 //!< Internal 12MHz RC oscillator as master source clock +#define SYSCLK_SRC_MAINCK_XTAL 6 //!< External crystal oscillator as master source clock +#define SYSCLK_SRC_MAINCK_BYPASS 7 //!< External bypass oscillator as master source clock +#define SYSCLK_SRC_PLLACK 8 //!< Use PLLACK as master source clock +#define SYSCLK_SRC_UPLLCK 9 //!< Use UPLLCK as master source clock +//@} + +//! \name Master Clock Prescalers (MCK) +//@{ +#define SYSCLK_PRES_1 PMC_MCKR_PRES_CLK_1 //!< Set master clock prescaler to 1 +#define SYSCLK_PRES_2 PMC_MCKR_PRES_CLK_2 //!< Set master clock prescaler to 2 +#define SYSCLK_PRES_4 PMC_MCKR_PRES_CLK_4 //!< Set master clock prescaler to 4 +#define SYSCLK_PRES_8 PMC_MCKR_PRES_CLK_8 //!< Set master clock prescaler to 8 +#define SYSCLK_PRES_16 PMC_MCKR_PRES_CLK_16 //!< Set master clock prescaler to 16 +#define SYSCLK_PRES_32 PMC_MCKR_PRES_CLK_32 //!< Set master clock prescaler to 32 +#define SYSCLK_PRES_64 PMC_MCKR_PRES_CLK_64 //!< Set master clock prescaler to 64 +#define SYSCLK_PRES_3 PMC_MCKR_PRES_CLK_3 //!< Set master clock prescaler to 3 +//@} + +//! \name USB Clock Sources +//@{ +#define USBCLK_SRC_PLL0 0 //!< Use PLLA +#define USBCLK_SRC_UPLL 1 //!< Use UPLL +//@} + +/** + * \def CONFIG_USBCLK_SOURCE + * \brief Configuration symbol for the USB generic clock source + * + * Sets the clock source to use for the USB. The source must also be properly + * configured. + * + * Define this to one of the \c USBCLK_SRC_xxx settings. Leave it undefined if + * USB is not required. + */ +#ifdef __DOXYGEN__ +# define CONFIG_USBCLK_SOURCE +#endif + +/** + * \def CONFIG_USBCLK_DIV + * \brief Configuration symbol for the USB generic clock divider setting + * + * Sets the clock division for the USB generic clock. If a USB clock source is + * selected with CONFIG_USBCLK_SOURCE, this configuration symbol must also be + * defined. + */ +#ifdef __DOXYGEN__ +# define CONFIG_USBCLK_DIV +#endif + + +extern void sysclk_enable_usb(void); +extern void sysclk_disable_usb(void); + +//! @} + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif /* CHIP_SYSCLK_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/tpaste.h b/src/HAL/DUE/usb/tpaste.h new file mode 100644 index 0000000..2ad3f27 --- /dev/null +++ b/src/HAL/DUE/usb/tpaste.h @@ -0,0 +1,105 @@ +/** + * \file + * + * \brief Preprocessor token pasting utils. + * + * Copyright (c) 2010-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _TPASTE_H_ +#define _TPASTE_H_ + +/** + * \defgroup group_sam_utils_tpaste Preprocessor - Token Paste + * + * \ingroup group_sam_utils + * + * \{ + */ + +/*! \name Token Paste + * + * Paste N preprocessing tokens together, these tokens being allowed to be \#defined. + * + * May be used only within macros with the tokens passed as arguments if the tokens are \#defined. + * + * For example, writing TPASTE2(U, WIDTH) within a macro \#defined by + * UTYPE(WIDTH) and invoked as UTYPE(UL_WIDTH) with UL_WIDTH \#defined as 32 is + * equivalent to writing U32. + */ +//! @{ +#define TPASTE2( a, b) a##b +#define TPASTE3( a, b, c) a##b##c +#define TPASTE4( a, b, c, d) a##b##c##d +#define TPASTE5( a, b, c, d, e) a##b##c##d##e +#define TPASTE6( a, b, c, d, e, f) a##b##c##d##e##f +#define TPASTE7( a, b, c, d, e, f, g) a##b##c##d##e##f##g +#define TPASTE8( a, b, c, d, e, f, g, h) a##b##c##d##e##f##g##h +#define TPASTE9( a, b, c, d, e, f, g, h, i) a##b##c##d##e##f##g##h##i +#define TPASTE10(a, b, c, d, e, f, g, h, i, j) a##b##c##d##e##f##g##h##i##j +//! @} + +/*! \name Absolute Token Paste + * + * Paste N preprocessing tokens together, these tokens being allowed to be \#defined. + * + * No restriction of use if the tokens are \#defined. + * + * For example, writing ATPASTE2(U, UL_WIDTH) anywhere with UL_WIDTH \#defined + * as 32 is equivalent to writing U32. + */ +//! @{ +#define ATPASTE2( a, b) TPASTE2( a, b) +#define ATPASTE3( a, b, c) TPASTE3( a, b, c) +#define ATPASTE4( a, b, c, d) TPASTE4( a, b, c, d) +#define ATPASTE5( a, b, c, d, e) TPASTE5( a, b, c, d, e) +#define ATPASTE6( a, b, c, d, e, f) TPASTE6( a, b, c, d, e, f) +#define ATPASTE7( a, b, c, d, e, f, g) TPASTE7( a, b, c, d, e, f, g) +#define ATPASTE8( a, b, c, d, e, f, g, h) TPASTE8( a, b, c, d, e, f, g, h) +#define ATPASTE9( a, b, c, d, e, f, g, h, i) TPASTE9( a, b, c, d, e, f, g, h, i) +#define ATPASTE10(a, b, c, d, e, f, g, h, i, j) TPASTE10(a, b, c, d, e, f, g, h, i, j) +//! @} + +/** + * \} + */ + +#endif // _TPASTE_H_ diff --git a/src/HAL/DUE/usb/udc.c b/src/HAL/DUE/usb/udc.c new file mode 100644 index 0000000..60bf0cf --- /dev/null +++ b/src/HAL/DUE/usb/udc.c @@ -0,0 +1,1149 @@ +/** + * \file + * + * \brief USB Device Controller (UDC) + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "udd.h" +#include "udc_desc.h" +#include "udi.h" +#include "udc.h" + +/** + * \ingroup udc_group + * \defgroup udc_group_interne Implementation of UDC + * + * Internal implementation + * @{ + */ + +//! \name Internal variables to manage the USB device +//! @{ + +//! Device status state (see enum usb_device_status in usb_protocol.h) +static le16_t udc_device_status; + +COMPILER_WORD_ALIGNED +//! Device interface setting value +static uint8_t udc_iface_setting = 0; + +//! Device Configuration number selected by the USB host +COMPILER_WORD_ALIGNED +static uint8_t udc_num_configuration = 0; + +//! Pointer on the selected speed device configuration +static udc_config_speed_t UDC_DESC_STORAGE *udc_ptr_conf; + +//! Pointer on interface descriptor used by SETUP request. +static usb_iface_desc_t UDC_DESC_STORAGE *udc_ptr_iface; + +//! @} + + +//! \name Internal structure to store the USB device main strings +//! @{ + +/** + * \brief Language ID of USB device (US ID by default) + */ +COMPILER_WORD_ALIGNED +static UDC_DESC_STORAGE usb_str_lgid_desc_t udc_string_desc_languageid = { + .desc.bLength = sizeof(usb_str_lgid_desc_t), + .desc.bDescriptorType = USB_DT_STRING, + .string = {LE16(USB_LANGID_EN_US)} +}; + +/** + * \brief USB device manufacture name storage + * String is allocated only if USB_DEVICE_MANUFACTURE_NAME is declared + * by usb application configuration + */ +#ifdef USB_DEVICE_MANUFACTURE_NAME +static uint8_t udc_string_manufacturer_name[] = USB_DEVICE_MANUFACTURE_NAME; +# define USB_DEVICE_MANUFACTURE_NAME_SIZE \ + (sizeof(udc_string_manufacturer_name)-1) +#else +# define USB_DEVICE_MANUFACTURE_NAME_SIZE 0 +#endif + +/** + * \brief USB device product name storage + * String is allocated only if USB_DEVICE_PRODUCT_NAME is declared + * by usb application configuration + */ +#ifdef USB_DEVICE_PRODUCT_NAME +static uint8_t udc_string_product_name[] = USB_DEVICE_PRODUCT_NAME; +# define USB_DEVICE_PRODUCT_NAME_SIZE (sizeof(udc_string_product_name)-1) +#else +# define USB_DEVICE_PRODUCT_NAME_SIZE 0 +#endif + +/** + * \brief Get USB device serial number + * + * Use the define USB_DEVICE_SERIAL_NAME to set static serial number. + * + * For dynamic serial number set the define USB_DEVICE_GET_SERIAL_NAME_POINTER + * to a suitable pointer. This will also require the serial number length + * define USB_DEVICE_GET_SERIAL_NAME_LENGTH. + */ +#if defined USB_DEVICE_GET_SERIAL_NAME_POINTER + static const uint8_t *udc_get_string_serial_name(void) + { + return (const uint8_t *)USB_DEVICE_GET_SERIAL_NAME_POINTER; + } +# define USB_DEVICE_SERIAL_NAME_SIZE \ + USB_DEVICE_GET_SERIAL_NAME_LENGTH +#elif defined USB_DEVICE_SERIAL_NAME + static const uint8_t *udc_get_string_serial_name(void) + { + return (const uint8_t *)USB_DEVICE_SERIAL_NAME; + } +# define USB_DEVICE_SERIAL_NAME_SIZE \ + (sizeof(USB_DEVICE_SERIAL_NAME)-1) +#else +# define USB_DEVICE_SERIAL_NAME_SIZE 0 +#endif + +/** + * \brief USB device string descriptor + * Structure used to transfer ASCII strings to USB String descriptor structure. + */ +struct udc_string_desc_t { + usb_str_desc_t header; + le16_t string[Max(Max(USB_DEVICE_MANUFACTURE_NAME_SIZE, \ + USB_DEVICE_PRODUCT_NAME_SIZE), USB_DEVICE_SERIAL_NAME_SIZE)]; +}; +COMPILER_WORD_ALIGNED +static UDC_DESC_STORAGE struct udc_string_desc_t udc_string_desc = { + .header.bDescriptorType = USB_DT_STRING +}; +//! @} + +usb_iface_desc_t UDC_DESC_STORAGE *udc_get_interface_desc(void) +{ + return udc_ptr_iface; +} + +/** + * \brief Returns a value to check the end of USB Configuration descriptor + * + * \return address after the last byte of USB Configuration descriptor + */ +static usb_conf_desc_t UDC_DESC_STORAGE *udc_get_eof_conf(void) +{ + return (UDC_DESC_STORAGE usb_conf_desc_t *) ((uint8_t *) + udc_ptr_conf->desc + + le16_to_cpu(udc_ptr_conf->desc->wTotalLength)); +} + +#if (0!=USB_DEVICE_MAX_EP) +/** + * \brief Search specific descriptor in global interface descriptor + * + * \param desc Address of interface descriptor + * or previous specific descriptor found + * \param desc_id Descriptor ID to search + * + * \return address of specific descriptor found + * \return NULL if it is the end of global interface descriptor + */ +static usb_conf_desc_t UDC_DESC_STORAGE *udc_next_desc_in_iface(usb_conf_desc_t + UDC_DESC_STORAGE * desc, uint8_t desc_id) +{ + usb_conf_desc_t UDC_DESC_STORAGE *ptr_eof_desc; + + ptr_eof_desc = udc_get_eof_conf(); + // Go to next descriptor + desc = (UDC_DESC_STORAGE usb_conf_desc_t *) ((uint8_t *) desc + + desc->bLength); + // Check the end of configuration descriptor + while (ptr_eof_desc > desc) { + // If new interface descriptor is found, + // then it is the end of the current global interface descriptor + if (USB_DT_INTERFACE == desc->bDescriptorType) { + break; // End of global interface descriptor + } + if (desc_id == desc->bDescriptorType) { + return desc; // Specific descriptor found + } + // Go to next descriptor + desc = (UDC_DESC_STORAGE usb_conf_desc_t *) ((uint8_t *) desc + + desc->bLength); + } + return NULL; // No specific descriptor found +} +#endif + +/** + * \brief Search an interface descriptor + * This routine updates the internal pointer udc_ptr_iface. + * + * \param iface_num Interface number to find in Configuration Descriptor + * \param setting_num Setting number of interface to find + * + * \return 1 if found or 0 if not found + */ +static bool udc_update_iface_desc(uint8_t iface_num, uint8_t setting_num) +{ + usb_conf_desc_t UDC_DESC_STORAGE *ptr_end_desc; + + if (0 == udc_num_configuration) { + return false; + } + + if (iface_num >= udc_ptr_conf->desc->bNumInterfaces) { + return false; + } + + // Start at the beginning of configuration descriptor + udc_ptr_iface = (UDC_DESC_STORAGE usb_iface_desc_t *) + udc_ptr_conf->desc; + + // Check the end of configuration descriptor + ptr_end_desc = udc_get_eof_conf(); + while (ptr_end_desc > + (UDC_DESC_STORAGE usb_conf_desc_t *) udc_ptr_iface) { + if (USB_DT_INTERFACE == udc_ptr_iface->bDescriptorType) { + // A interface descriptor is found + // Check interface and alternate setting number + if ((iface_num == udc_ptr_iface->bInterfaceNumber) && + (setting_num == + udc_ptr_iface->bAlternateSetting)) { + return true; // Interface found + } + } + // Go to next descriptor + udc_ptr_iface = (UDC_DESC_STORAGE usb_iface_desc_t *) ( + (uint8_t *) udc_ptr_iface + + udc_ptr_iface->bLength); + } + return false; // Interface not found +} + +/** + * \brief Disables an usb device interface (UDI) + * This routine call the UDI corresponding to interface number + * + * \param iface_num Interface number to disable + * + * \return 1 if it is done or 0 if interface is not found + */ +static bool udc_iface_disable(uint8_t iface_num) +{ + udi_api_t UDC_DESC_STORAGE *udi_api; + + // Select first alternate setting of the interface + // to update udc_ptr_iface before call iface->getsetting() + if (!udc_update_iface_desc(iface_num, 0)) { + return false; + } + + // Select the interface with the current alternate setting + udi_api = udc_ptr_conf->udi_apis[iface_num]; + +#if (0!=USB_DEVICE_MAX_EP) + if (!udc_update_iface_desc(iface_num, udi_api->getsetting())) { + return false; + } + + // Start at the beginning of interface descriptor + { + usb_ep_desc_t UDC_DESC_STORAGE *ep_desc; + ep_desc = (UDC_DESC_STORAGE usb_ep_desc_t *) udc_ptr_iface; + while (1) { + // Search Endpoint descriptor included in global interface descriptor + ep_desc = (UDC_DESC_STORAGE usb_ep_desc_t *) + udc_next_desc_in_iface((UDC_DESC_STORAGE + usb_conf_desc_t *) + ep_desc, USB_DT_ENDPOINT); + if (NULL == ep_desc) { + break; + } + // Free the endpoint used by the interface + udd_ep_free(ep_desc->bEndpointAddress); + } + } +#endif + + // Disable interface + udi_api->disable(); + return true; +} + +/** + * \brief Enables an usb device interface (UDI) + * This routine calls the UDI corresponding + * to the interface and setting number. + * + * \param iface_num Interface number to enable + * \param setting_num Setting number to enable + * + * \return 1 if it is done or 0 if interface is not found + */ +static bool udc_iface_enable(uint8_t iface_num, uint8_t setting_num) +{ + // Select the interface descriptor + if (!udc_update_iface_desc(iface_num, setting_num)) { + return false; + } + +#if (0!=USB_DEVICE_MAX_EP) + usb_ep_desc_t UDC_DESC_STORAGE *ep_desc; + + // Start at the beginning of the global interface descriptor + ep_desc = (UDC_DESC_STORAGE usb_ep_desc_t *) udc_ptr_iface; + while (1) { + // Search Endpoint descriptor included in the global interface descriptor + ep_desc = (UDC_DESC_STORAGE usb_ep_desc_t *) + udc_next_desc_in_iface((UDC_DESC_STORAGE + usb_conf_desc_t *) ep_desc, + USB_DT_ENDPOINT); + if (NULL == ep_desc) + break; + // Alloc the endpoint used by the interface + if (!udd_ep_alloc(ep_desc->bEndpointAddress, + ep_desc->bmAttributes, + le16_to_cpu + (ep_desc->wMaxPacketSize))) { + return false; + } + } +#endif + // Enable the interface + return udc_ptr_conf->udi_apis[iface_num]->enable(); +} + +/*! \brief Start the USB Device stack + */ +void udc_start(void) +{ + udd_enable(); +} + +/*! \brief Stop the USB Device stack + */ +void udc_stop(void) +{ + udd_disable(); + udc_reset(); +} + +/** + * \brief Reset the current configuration of the USB device, + * This routines can be called by UDD when a RESET on the USB line occurs. + */ +void udc_reset(void) +{ + uint8_t iface_num; + + if (udc_num_configuration) { + for (iface_num = 0; + iface_num < udc_ptr_conf->desc->bNumInterfaces; + iface_num++) { + udc_iface_disable(iface_num); + } + } + udc_num_configuration = 0; +#if (USB_CONFIG_ATTR_REMOTE_WAKEUP \ + == (USB_DEVICE_ATTR & USB_CONFIG_ATTR_REMOTE_WAKEUP)) + if (CPU_TO_LE16(USB_DEV_STATUS_REMOTEWAKEUP) & udc_device_status) { + // Remote wakeup is enabled then disable it + UDC_REMOTEWAKEUP_DISABLE(); + } +#endif + udc_device_status = +#if (USB_DEVICE_ATTR & USB_CONFIG_ATTR_SELF_POWERED) + CPU_TO_LE16(USB_DEV_STATUS_SELF_POWERED); +#else + CPU_TO_LE16(USB_DEV_STATUS_BUS_POWERED); +#endif +} + +void udc_sof_notify(void) +{ + uint8_t iface_num; + + if (udc_num_configuration) { + for (iface_num = 0; + iface_num < udc_ptr_conf->desc->bNumInterfaces; + iface_num++) { + if (udc_ptr_conf->udi_apis[iface_num]->sof_notify != NULL) { + udc_ptr_conf->udi_apis[iface_num]->sof_notify(); + } + } + } +} + +/** + * \brief Standard device request to get device status + * + * \return true if success + */ +static bool udc_req_std_dev_get_status(void) +{ + if (udd_g_ctrlreq.req.wLength != sizeof(udc_device_status)) { + return false; + } + + udd_set_setup_payload( (uint8_t *) & udc_device_status, + sizeof(udc_device_status)); + return true; +} + +#if (0!=USB_DEVICE_MAX_EP) +/** + * \brief Standard endpoint request to get endpoint status + * + * \return true if success + */ +static bool udc_req_std_ep_get_status(void) +{ + static le16_t udc_ep_status; + + if (udd_g_ctrlreq.req.wLength != sizeof(udc_ep_status)) { + return false; + } + + udc_ep_status = udd_ep_is_halted(udd_g_ctrlreq.req. + wIndex & 0xFF) ? CPU_TO_LE16(USB_EP_STATUS_HALTED) : 0; + + udd_set_setup_payload( (uint8_t *) & udc_ep_status, + sizeof(udc_ep_status)); + return true; +} +#endif + +/** + * \brief Standard device request to change device status + * + * \return true if success + */ +static bool udc_req_std_dev_clear_feature(void) +{ + if (udd_g_ctrlreq.req.wLength) { + return false; + } + + if (udd_g_ctrlreq.req.wValue == USB_DEV_FEATURE_REMOTE_WAKEUP) { + udc_device_status &= CPU_TO_LE16(~(uint32_t)USB_DEV_STATUS_REMOTEWAKEUP); +#if (USB_CONFIG_ATTR_REMOTE_WAKEUP \ + == (USB_DEVICE_ATTR & USB_CONFIG_ATTR_REMOTE_WAKEUP)) + UDC_REMOTEWAKEUP_DISABLE(); +#endif + return true; + } + return false; +} + +#if (0!=USB_DEVICE_MAX_EP) +/** + * \brief Standard endpoint request to clear endpoint feature + * + * \return true if success + */ +static bool udc_req_std_ep_clear_feature(void) +{ + if (udd_g_ctrlreq.req.wLength) { + return false; + } + + if (udd_g_ctrlreq.req.wValue == USB_EP_FEATURE_HALT) { + return udd_ep_clear_halt(udd_g_ctrlreq.req.wIndex & 0xFF); + } + return false; +} +#endif + +/** + * \brief Standard device request to set a feature + * + * \return true if success + */ +static bool udc_req_std_dev_set_feature(void) +{ + if (udd_g_ctrlreq.req.wLength) { + return false; + } + + switch (udd_g_ctrlreq.req.wValue) { + + case USB_DEV_FEATURE_REMOTE_WAKEUP: +#if (USB_CONFIG_ATTR_REMOTE_WAKEUP \ + == (USB_DEVICE_ATTR & USB_CONFIG_ATTR_REMOTE_WAKEUP)) + udc_device_status |= CPU_TO_LE16(USB_DEV_STATUS_REMOTEWAKEUP); + UDC_REMOTEWAKEUP_ENABLE(); + return true; +#else + return false; +#endif + +#ifdef USB_DEVICE_HS_SUPPORT + case USB_DEV_FEATURE_TEST_MODE: + if (!udd_is_high_speed()) { + break; + } + if (udd_g_ctrlreq.req.wIndex & 0xFF) { + break; + } + // Unconfigure the device, terminating all ongoing requests + udc_reset(); + switch ((udd_g_ctrlreq.req.wIndex >> 8) & 0xFF) { + case USB_DEV_TEST_MODE_J: + udd_g_ctrlreq.callback = udd_test_mode_j; + return true; + + case USB_DEV_TEST_MODE_K: + udd_g_ctrlreq.callback = udd_test_mode_k; + return true; + + case USB_DEV_TEST_MODE_SE0_NAK: + udd_g_ctrlreq.callback = udd_test_mode_se0_nak; + return true; + + case USB_DEV_TEST_MODE_PACKET: + udd_g_ctrlreq.callback = udd_test_mode_packet; + return true; + + case USB_DEV_TEST_MODE_FORCE_ENABLE: // Only for downstream facing hub ports + default: + break; + } + break; +#endif + default: + break; + } + return false; +} + +/** + * \brief Standard endpoint request to halt an endpoint + * + * \return true if success + */ +#if (0!=USB_DEVICE_MAX_EP) +static bool udc_req_std_ep_set_feature(void) +{ + if (udd_g_ctrlreq.req.wLength) { + return false; + } + if (udd_g_ctrlreq.req.wValue == USB_EP_FEATURE_HALT) { + udd_ep_abort(udd_g_ctrlreq.req.wIndex & 0xFF); + return udd_ep_set_halt(udd_g_ctrlreq.req.wIndex & 0xFF); + } + return false; +} +#endif + +/** + * \brief Change the address of device + * Callback called at the end of request set address + */ +static void udc_valid_address(void) +{ + udd_set_address(udd_g_ctrlreq.req.wValue & 0x7F); +} + +/** + * \brief Standard device request to set device address + * + * \return true if success + */ +static bool udc_req_std_dev_set_address(void) +{ + if (udd_g_ctrlreq.req.wLength) { + return false; + } + + // The address must be changed at the end of setup request after the handshake + // then we use a callback to change address + udd_g_ctrlreq.callback = udc_valid_address; + return true; +} + +/** + * \brief Standard device request to get device string descriptor + * + * \return true if success + */ +static bool udc_req_std_dev_get_str_desc(void) +{ + uint8_t i; + const uint8_t *str; + uint8_t str_length = 0; + + // Link payload pointer to the string corresponding at request + switch (udd_g_ctrlreq.req.wValue & 0xFF) { + case 0: + udd_set_setup_payload((uint8_t *) &udc_string_desc_languageid, + sizeof(udc_string_desc_languageid)); + break; + +#ifdef USB_DEVICE_MANUFACTURE_NAME + case 1: + str_length = USB_DEVICE_MANUFACTURE_NAME_SIZE; + str = udc_string_manufacturer_name; + break; +#endif +#ifdef USB_DEVICE_PRODUCT_NAME + case 2: + str_length = USB_DEVICE_PRODUCT_NAME_SIZE; + str = udc_string_product_name; + break; +#endif +#if defined USB_DEVICE_SERIAL_NAME || defined USB_DEVICE_GET_SERIAL_NAME_POINTER + case 3: + str_length = USB_DEVICE_SERIAL_NAME_SIZE; + str = udc_get_string_serial_name(); + break; +#endif + default: +#ifdef UDC_GET_EXTRA_STRING + if (UDC_GET_EXTRA_STRING()) { + break; + } +#endif + return false; + } + + if (str_length) { + for(i = 0; i < str_length; i++) { + udc_string_desc.string[i] = cpu_to_le16((le16_t)str[i]); + } + + udc_string_desc.header.bLength = 2 + (str_length) * 2; + udd_set_setup_payload( + (uint8_t *) &udc_string_desc, + udc_string_desc.header.bLength); + } + + return true; +} + +/** + * \brief Standard device request to get descriptors about USB device + * + * \return true if success + */ +static bool udc_req_std_dev_get_descriptor(void) +{ + uint8_t conf_num; + + conf_num = udd_g_ctrlreq.req.wValue & 0xFF; + + // Check descriptor ID + switch ((uint8_t) (udd_g_ctrlreq.req.wValue >> 8)) { + case USB_DT_DEVICE: + // Device descriptor requested +#ifdef USB_DEVICE_HS_SUPPORT + if (!udd_is_high_speed()) { + udd_set_setup_payload( + (uint8_t *) udc_config.confdev_hs, + udc_config.confdev_hs->bLength); + } else +#endif + { + udd_set_setup_payload( + (uint8_t *) udc_config.confdev_lsfs, + udc_config.confdev_lsfs->bLength); + } + break; + + case USB_DT_CONFIGURATION: + // Configuration descriptor requested +#ifdef USB_DEVICE_HS_SUPPORT + if (udd_is_high_speed()) { + // HS descriptor + if (conf_num >= udc_config.confdev_hs-> + bNumConfigurations) { + return false; + } + udd_set_setup_payload( + (uint8_t *)udc_config.conf_hs[conf_num].desc, + le16_to_cpu(udc_config.conf_hs[conf_num].desc->wTotalLength)); + } else +#endif + { + // FS descriptor + if (conf_num >= udc_config.confdev_lsfs-> + bNumConfigurations) { + return false; + } + udd_set_setup_payload( + (uint8_t *)udc_config.conf_lsfs[conf_num].desc, + le16_to_cpu(udc_config.conf_lsfs[conf_num].desc->wTotalLength)); + } + ((usb_conf_desc_t *) udd_g_ctrlreq.payload)->bDescriptorType = + USB_DT_CONFIGURATION; + break; + +#ifdef USB_DEVICE_HS_SUPPORT + case USB_DT_DEVICE_QUALIFIER: + // Device qualifier descriptor requested + udd_set_setup_payload( (uint8_t *) udc_config.qualifier, + udc_config.qualifier->bLength); + break; + + case USB_DT_OTHER_SPEED_CONFIGURATION: + // Other configuration descriptor requested + if (!udd_is_high_speed()) { + // HS descriptor + if (conf_num >= udc_config.confdev_hs-> + bNumConfigurations) { + return false; + } + udd_set_setup_payload( + (uint8_t *)udc_config.conf_hs[conf_num].desc, + le16_to_cpu(udc_config.conf_hs[conf_num].desc->wTotalLength)); + } else { + // FS descriptor + if (conf_num >= udc_config.confdev_lsfs-> + bNumConfigurations) { + return false; + } + udd_set_setup_payload( + (uint8_t *)udc_config.conf_lsfs[conf_num].desc, + le16_to_cpu(udc_config.conf_lsfs[conf_num].desc->wTotalLength)); + } + ((usb_conf_desc_t *) udd_g_ctrlreq.payload)->bDescriptorType = + USB_DT_OTHER_SPEED_CONFIGURATION; + break; +#endif + + case USB_DT_BOS: + // Device BOS descriptor requested + if (udc_config.conf_bos == NULL) { + return false; + } + udd_set_setup_payload( (uint8_t *) udc_config.conf_bos, + udc_config.conf_bos->wTotalLength); + break; + + case USB_DT_STRING: + // String descriptor requested + if (!udc_req_std_dev_get_str_desc()) { + return false; + } + break; + + default: + // Unknown descriptor requested + return false; + } + // if the descriptor is larger than length requested, then reduce it + if (udd_g_ctrlreq.req.wLength < udd_g_ctrlreq.payload_size) { + udd_g_ctrlreq.payload_size = udd_g_ctrlreq.req.wLength; + } + return true; +} + +/** + * \brief Standard device request to get configuration number + * + * \return true if success + */ +static bool udc_req_std_dev_get_configuration(void) +{ + if (udd_g_ctrlreq.req.wLength != 1) { + return false; + } + + udd_set_setup_payload(&udc_num_configuration,1); + return true; +} + +/** + * \brief Standard device request to enable a configuration + * + * \return true if success + */ +static bool udc_req_std_dev_set_configuration(void) +{ + uint8_t iface_num; + + // Check request length + if (udd_g_ctrlreq.req.wLength) { + return false; + } + // Authorize configuration only if the address is valid + if (!udd_getaddress()) { + return false; + } + // Check the configuration number requested +#ifdef USB_DEVICE_HS_SUPPORT + if (udd_is_high_speed()) { + // HS descriptor + if ((udd_g_ctrlreq.req.wValue & 0xFF) > + udc_config.confdev_hs->bNumConfigurations) { + return false; + } + } else +#endif + { + // FS descriptor + if ((udd_g_ctrlreq.req.wValue & 0xFF) > + udc_config.confdev_lsfs->bNumConfigurations) { + return false; + } + } + + // Reset current configuration + udc_reset(); + + // Enable new configuration + udc_num_configuration = udd_g_ctrlreq.req.wValue & 0xFF; + if (udc_num_configuration == 0) { + return true; // Default empty configuration requested + } + // Update pointer of the configuration descriptor +#ifdef USB_DEVICE_HS_SUPPORT + if (udd_is_high_speed()) { + // HS descriptor + udc_ptr_conf = &udc_config.conf_hs[udc_num_configuration - 1]; + } else +#endif + { + // FS descriptor + udc_ptr_conf = &udc_config.conf_lsfs[udc_num_configuration - 1]; + } + // Enable all interfaces of the selected configuration + for (iface_num = 0; iface_num < udc_ptr_conf->desc->bNumInterfaces; + iface_num++) { + if (!udc_iface_enable(iface_num, 0)) { + return false; + } + } + return true; +} + +/** + * \brief Standard interface request + * to get the alternate setting number of an interface + * + * \return true if success + */ +static bool udc_req_std_iface_get_setting(void) +{ + uint8_t iface_num; + udi_api_t UDC_DESC_STORAGE *udi_api; + + if (udd_g_ctrlreq.req.wLength != 1) { + return false; // Error in request + } + if (!udc_num_configuration) { + return false; // The device is not is configured state yet + } + + // Check the interface number included in the request + iface_num = udd_g_ctrlreq.req.wIndex & 0xFF; + if (iface_num >= udc_ptr_conf->desc->bNumInterfaces) { + return false; + } + + // Select first alternate setting of the interface to update udc_ptr_iface + // before call iface->getsetting() + if (!udc_update_iface_desc(iface_num, 0)) { + return false; + } + // Get alternate setting from UDI + udi_api = udc_ptr_conf->udi_apis[iface_num]; + udc_iface_setting = udi_api->getsetting(); + + // Link value to payload pointer of request + udd_set_setup_payload(&udc_iface_setting,1); + return true; +} + +/** + * \brief Standard interface request + * to set an alternate setting of an interface + * + * \return true if success + */ +static bool udc_req_std_iface_set_setting(void) +{ + uint8_t iface_num, setting_num; + + if (udd_g_ctrlreq.req.wLength) { + return false; // Error in request + } + if (!udc_num_configuration) { + return false; // The device is not is configured state yet + } + + iface_num = udd_g_ctrlreq.req.wIndex & 0xFF; + setting_num = udd_g_ctrlreq.req.wValue & 0xFF; + + // Disable current setting + if (!udc_iface_disable(iface_num)) { + return false; + } + + // Enable new setting + return udc_iface_enable(iface_num, setting_num); +} + +/** + * \brief Main routine to manage the standard USB SETUP request + * + * \return true if the request is supported + */ +static bool udc_reqstd(void) +{ + if (Udd_setup_is_in()) { + // GET Standard Requests + if (udd_g_ctrlreq.req.wLength == 0) { + return false; // Error for USB host + } + + if (USB_REQ_RECIP_DEVICE == Udd_setup_recipient()) { + // Standard Get Device request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_GET_STATUS: + return udc_req_std_dev_get_status(); + case USB_REQ_GET_DESCRIPTOR: + return udc_req_std_dev_get_descriptor(); + case USB_REQ_GET_CONFIGURATION: + return udc_req_std_dev_get_configuration(); + default: + break; + } + } + + if (USB_REQ_RECIP_INTERFACE == Udd_setup_recipient()) { + // Standard Get Interface request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_GET_INTERFACE: + return udc_req_std_iface_get_setting(); + default: + break; + } + } +#if (0!=USB_DEVICE_MAX_EP) + if (USB_REQ_RECIP_ENDPOINT == Udd_setup_recipient()) { + // Standard Get Endpoint request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_GET_STATUS: + return udc_req_std_ep_get_status(); + default: + break; + } + } +#endif + } else { + // SET Standard Requests + if (USB_REQ_RECIP_DEVICE == Udd_setup_recipient()) { + // Standard Set Device request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_SET_ADDRESS: + return udc_req_std_dev_set_address(); + case USB_REQ_CLEAR_FEATURE: + return udc_req_std_dev_clear_feature(); + case USB_REQ_SET_FEATURE: + return udc_req_std_dev_set_feature(); + case USB_REQ_SET_CONFIGURATION: + return udc_req_std_dev_set_configuration(); + case USB_REQ_SET_DESCRIPTOR: + /* Not supported (defined as optional by the USB 2.0 spec) */ + break; + default: + break; + } + } + + if (USB_REQ_RECIP_INTERFACE == Udd_setup_recipient()) { + // Standard Set Interface request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_SET_INTERFACE: + return udc_req_std_iface_set_setting(); + default: + break; + } + } +#if (0!=USB_DEVICE_MAX_EP) + if (USB_REQ_RECIP_ENDPOINT == Udd_setup_recipient()) { + // Standard Set Endpoint request + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_CLEAR_FEATURE: + return udc_req_std_ep_clear_feature(); + case USB_REQ_SET_FEATURE: + return udc_req_std_ep_set_feature(); + default: + break; + } + } +#endif + } + return false; +} + +/** + * \brief Send the SETUP interface request to UDI + * + * \return true if the request is supported + */ +static bool udc_req_iface(void) +{ + uint8_t iface_num; + udi_api_t UDC_DESC_STORAGE *udi_api; + + if (0 == udc_num_configuration) { + return false; // The device is not is configured state yet + } + // Check interface number + iface_num = udd_g_ctrlreq.req.wIndex & 0xFF; + if (iface_num >= udc_ptr_conf->desc->bNumInterfaces) { + return false; + } + + //* To update udc_ptr_iface with the selected interface in request + // Select first alternate setting of interface to update udc_ptr_iface + // before calling udi_api->getsetting() + if (!udc_update_iface_desc(iface_num, 0)) { + return false; + } + // Select the interface with the current alternate setting + udi_api = udc_ptr_conf->udi_apis[iface_num]; + if (!udc_update_iface_desc(iface_num, udi_api->getsetting())) { + return false; + } + + // Send the SETUP request to the UDI corresponding to the interface number + return udi_api->setup(); +} + +/** + * \brief Send the SETUP interface request to UDI + * + * \return true if the request is supported + */ +static bool udc_req_ep(void) +{ + uint8_t iface_num; + udi_api_t UDC_DESC_STORAGE *udi_api; + + if (0 == udc_num_configuration) { + return false; // The device is not is configured state yet + } + // Send this request on all enabled interfaces + iface_num = udd_g_ctrlreq.req.wIndex & 0xFF; + for (iface_num = 0; iface_num < udc_ptr_conf->desc->bNumInterfaces; + iface_num++) { + // Select the interface with the current alternate setting + udi_api = udc_ptr_conf->udi_apis[iface_num]; + if (!udc_update_iface_desc(iface_num, udi_api->getsetting())) { + return false; + } + + // Send the SETUP request to the UDI + if (udi_api->setup()) { + return true; + } + } + return false; +} + +/** + * \brief Main routine to manage the USB SETUP request. + * + * This function parses a USB SETUP request and submits an appropriate + * response back to the host or, in the case of SETUP OUT requests + * with data, sets up a buffer for receiving the data payload. + * + * The main standard requests defined by the USB 2.0 standard are handled + * internally. The interface requests are sent to UDI, and the specific request + * sent to a specific application callback. + * + * \return true if the request is supported, else the request is stalled by UDD + */ +bool udc_process_setup(void) +{ + // By default no data (receive/send) and no callbacks registered + udd_g_ctrlreq.payload_size = 0; + udd_g_ctrlreq.callback = NULL; + udd_g_ctrlreq.over_under_run = NULL; + + if (Udd_setup_is_in()) { + if (udd_g_ctrlreq.req.wLength == 0) { + return false; // Error from USB host + } + } + + // If standard request then try to decode it in UDC + if (Udd_setup_type() == USB_REQ_TYPE_STANDARD) { + if (udc_reqstd()) { + return true; + } + } + + // If interface request then try to decode it in UDI + if (Udd_setup_recipient() == USB_REQ_RECIP_INTERFACE) { + if (udc_req_iface()) { + return true; + } + } + + // If endpoint request then try to decode it in UDI + if (Udd_setup_recipient() == USB_REQ_RECIP_ENDPOINT) { + if (udc_req_ep()) { + return true; + } + } + + // Here SETUP request unknown by UDC and UDIs +#ifdef USB_DEVICE_SPECIFIC_REQUEST + // Try to decode it in specific callback + return USB_DEVICE_SPECIFIC_REQUEST(); // Ex: Vendor request,... +#else + return false; +#endif +} + +//! @} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/udc.h b/src/HAL/DUE/usb/udc.h new file mode 100644 index 0000000..8d92eb5 --- /dev/null +++ b/src/HAL/DUE/usb/udc.h @@ -0,0 +1,697 @@ +/** + * \file + * + * \brief Interface of the USB Device Controller (UDC) + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDC_H_ +#define _UDC_H_ + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "udc_desc.h" +#include "udd.h" + +#if USB_DEVICE_VENDOR_ID == 0 +# error USB_DEVICE_VENDOR_ID cannot be equal to 0 +#endif + +#if USB_DEVICE_PRODUCT_ID == 0 +# error USB_DEVICE_PRODUCT_ID cannot be equal to 0 +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \ingroup usb_device_group + * \defgroup udc_group USB Device Controller (UDC) + * + * The UDC provides a high-level abstraction of the usb device. + * You can use these functions to control the main device state + * (start/attach/wakeup). + * + * \section USB_DEVICE_CONF USB Device Custom configuration + * The following USB Device configuration must be included in the conf_usb.h + * file of the application. + * + * USB_DEVICE_VENDOR_ID (Word)
+ * Vendor ID provided by USB org (ATMEL 0x03EB). + * + * USB_DEVICE_PRODUCT_ID (Word)
+ * Product ID (Referenced in usb_atmel.h). + * + * USB_DEVICE_MAJOR_VERSION (Byte)
+ * Major version of the device + * + * USB_DEVICE_MINOR_VERSION (Byte)
+ * Minor version of the device + * + * USB_DEVICE_MANUFACTURE_NAME (string)
+ * ASCII name for the manufacture + * + * USB_DEVICE_PRODUCT_NAME (string)
+ * ASCII name for the product + * + * USB_DEVICE_SERIAL_NAME (string)
+ * ASCII name to enable and set a serial number + * + * USB_DEVICE_POWER (Numeric)
+ * (unit mA) Maximum device power + * + * USB_DEVICE_ATTR (Byte)
+ * USB attributes available: + * - USB_CONFIG_ATTR_SELF_POWERED + * - USB_CONFIG_ATTR_REMOTE_WAKEUP + * Note: if remote wake enabled then defines remotewakeup callbacks, + * see Table 5-2. External API from UDC - Callback + * + * USB_DEVICE_LOW_SPEED (Only defined)
+ * Force the USB Device to run in low speed + * + * USB_DEVICE_HS_SUPPORT (Only defined)
+ * Authorize the USB Device to run in high speed + * + * USB_DEVICE_MAX_EP (Byte)
+ * Define the maximum endpoint number used by the USB Device.
+ * This one is already defined in UDI default configuration. + * Ex: + * - When endpoint control 0x00, endpoint 0x01 and + * endpoint 0x82 is used then USB_DEVICE_MAX_EP=2 + * - When only endpoint control 0x00 is used then USB_DEVICE_MAX_EP=0 + * - When endpoint 0x01 and endpoint 0x81 is used then USB_DEVICE_MAX_EP=1
+ * (configuration not possible on USBB interface) + * @{ + */ + +/** + * \brief Authorizes the VBUS event + * + * \return true, if the VBUS monitoring is possible. + * + * \section udc_vbus_monitoring VBus monitoring used cases + * + * The VBus monitoring is used only for USB SELF Power application. + * + * - By default the USB device is automatically attached when Vbus is high + * or when USB is start for devices without internal Vbus monitoring. + * conf_usb.h file does not contains define USB_DEVICE_ATTACH_AUTO_DISABLE. + * \code //#define USB_DEVICE_ATTACH_AUTO_DISABLE \endcode + * + * - Add custom VBUS monitoring. conf_usb.h file contains define + * USB_DEVICE_ATTACH_AUTO_DISABLE: + * \code #define USB_DEVICE_ATTACH_AUTO_DISABLE \endcode + * User C file contains: + * \code + // Authorize VBUS monitoring + if (!udc_include_vbus_monitoring()) { + // Implement custom VBUS monitoring via GPIO or other + } + Event_VBUS_present() // VBUS interrupt or GPIO interrupt or other + { + // Attach USB Device + udc_attach(); + } +\endcode + * + * - Case of battery charging. conf_usb.h file contains define + * USB_DEVICE_ATTACH_AUTO_DISABLE: + * \code #define USB_DEVICE_ATTACH_AUTO_DISABLE \endcode + * User C file contains: + * \code + Event VBUS present() // VBUS interrupt or GPIO interrupt or .. + { + // Authorize battery charging, but wait key press to start USB. + } + Event Key press() + { + // Stop batteries charging + // Start USB + udc_attach(); + } +\endcode + */ +static inline bool udc_include_vbus_monitoring(void) +{ + return udd_include_vbus_monitoring(); +} + +/*! \brief Start the USB Device stack + */ +void udc_start(void); + +/*! \brief Stop the USB Device stack + */ +void udc_stop(void); + +/** + * \brief Attach device to the bus when possible + * + * \warning If a VBus control is included in driver, + * then it will attach device when an acceptable Vbus + * level from the host is detected. + */ +static inline void udc_attach(void) +{ + udd_attach(); +} + + +/** + * \brief Detaches the device from the bus + * + * The driver must remove pull-up on USB line D- or D+. + */ +static inline void udc_detach(void) +{ + udd_detach(); +} + + +/*! \brief The USB driver sends a resume signal called \e "Upstream Resume" + * This is authorized only when the remote wakeup feature is enabled by host. + */ +static inline void udc_remotewakeup(void) +{ + udd_send_remotewakeup(); +} + + +/** + * \brief Returns a pointer on the current interface descriptor + * + * \return pointer on the current interface descriptor. + */ +usb_iface_desc_t UDC_DESC_STORAGE *udc_get_interface_desc(void); + +//@} + +/** + * \ingroup usb_group + * \defgroup usb_device_group USB Stack Device + * + * This module includes USB Stack Device implementation. + * The stack is divided in three parts: + * - USB Device Controller (UDC) provides USB chapter 9 compliance + * - USB Device Interface (UDI) provides USB Class compliance + * - USB Device Driver (UDD) provides USB Driver for each Atmel MCU + + * Many USB Device applications can be implemented on Atmel MCU. + * Atmel provides many application notes for different applications: + * - AVR4900, provides general information about Device Stack + * - AVR4901, explains how to create a new class + * - AVR4902, explains how to create a composite device + * - AVR49xx, all device classes provided in ASF have an application note + * + * A basic USB knowledge is required to understand the USB Device + * Class application notes (HID,MS,CDC,PHDC,...). + * Then, to create an USB device with + * only one class provided by ASF, refer directly to the application note + * corresponding to this USB class. The USB Device application note for + * New Class and Composite is dedicated to advanced USB users. + * + * @{ + */ + +//! @} + +#ifdef __cplusplus +} +#endif + +/** + * \ingroup udc_group + * \defgroup udc_basic_use_case_setup_prereq USB Device Controller (UDC) - Prerequisites + * Common prerequisites for all USB devices. + * + * This module is based on USB device stack full interrupt driven, and supporting + * \ref sleepmgr_group sleepmgr. For AVR and SAM3/4 devices the \ref clk_group clock services + * is supported. For SAMD devices the \ref asfdoc_sam0_system_clock_group clock driver is supported. + * + * The following procedure must be executed to setup the project correctly: + * - Specify the clock configuration: + * - XMEGA USB devices need 48MHz clock input.\n + * XMEGA USB devices need CPU frequency higher than 12MHz.\n + * You can use either an internal RC48MHz auto calibrated by Start of Frames + * or an external OSC. + * - UC3 and SAM3/4 devices without USB high speed support need 48MHz clock input.\n + * You must use a PLL and an external OSC. + * - UC3 and SAM3/4 devices with USB high speed support need 12MHz clock input.\n + * You must use an external OSC. + * - UC3 devices with USBC hardware need CPU frequency higher than 25MHz. + * - SAMD devices without USB high speed support need 48MHz clock input.\n + * You should use DFLL with USBCRM. + * - In conf_board.h, the define CONF_BOARD_USB_PORT must be added to enable USB lines. + * (Not mandatory for all boards) + * - Enable interrupts + * - Initialize the clock service + * + * The usage of \ref sleepmgr_group sleepmgr service is optional, but recommended to reduce power + * consumption: + * - Initialize the sleep manager service + * - Activate sleep mode when the application is in IDLE state + * + * \subpage udc_conf_clock. + * + * for AVR and SAM3/4 devices, add to the initialization code: + * \code + sysclk_init(); + irq_initialize_vectors(); + cpu_irq_enable(); + board_init(); + sleepmgr_init(); // Optional +\endcode + * + * For SAMD devices, add to the initialization code: + * \code + system_init(); + irq_initialize_vectors(); + cpu_irq_enable(); + sleepmgr_init(); // Optional +\endcode + * Add to the main IDLE loop: + * \code + sleepmgr_enter_sleep(); // Optional +\endcode + * + */ + +/** + * \ingroup udc_group + * \defgroup udc_basic_use_case_setup_code USB Device Controller (UDC) - Example code + * Common example code for all USB devices. + * + * Content of conf_usb.h: + * \code + #define USB_DEVICE_VENDOR_ID 0x03EB + #define USB_DEVICE_PRODUCT_ID 0xXXXX + #define USB_DEVICE_MAJOR_VERSION 1 + #define USB_DEVICE_MINOR_VERSION 0 + #define USB_DEVICE_POWER 100 + #define USB_DEVICE_ATTR USB_CONFIG_ATTR_BUS_POWERED +\endcode + * + * Add to application C-file: + * \code + void usb_init(void) + { + udc_start(); + } +\endcode + */ + +/** + * \ingroup udc_group + * \defgroup udc_basic_use_case_setup_flow USB Device Controller (UDC) - Workflow + * Common workflow for all USB devices. + * + * -# Ensure that conf_usb.h is available and contains the following configuration + * which is the main USB device configuration: + * - \code // Vendor ID provided by USB org (ATMEL 0x03EB) + #define USB_DEVICE_VENDOR_ID 0x03EB // Type Word + // Product ID (Atmel PID referenced in usb_atmel.h) + #define USB_DEVICE_PRODUCT_ID 0xXXXX // Type Word + // Major version of the device + #define USB_DEVICE_MAJOR_VERSION 1 // Type Byte + // Minor version of the device + #define USB_DEVICE_MINOR_VERSION 0 // Type Byte + // Maximum device power (mA) + #define USB_DEVICE_POWER 100 // Type 9-bits + // USB attributes to enable features + #define USB_DEVICE_ATTR USB_CONFIG_ATTR_BUS_POWERED // Flags \endcode + * -# Call the USB device stack start function to enable stack and start USB: + * - \code udc_start(); \endcode + * \note In case of USB dual roles (Device and Host) managed through USB OTG connector + * (USB ID pin), the call of udc_start() must be removed and replaced by uhc_start(). + * SeRefer to "AVR4950 section 6.1 Dual roles" for further information about dual roles. + */ + +/** + * \page udc_conf_clock conf_clock.h examples with USB support + * + * Content of XMEGA conf_clock.h: + * \code + // Configuration based on internal RC: + // USB clock need of 48Mhz + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_RCOSC + #define CONFIG_OSC_RC32_CAL 48000000UL + #define CONFIG_OSC_AUTOCAL_RC32MHZ_REF_OSC OSC_ID_USBSOF + // CPU clock need of clock > 12MHz to run with USB (Here 24MHz) + #define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_RC32MHZ + #define CONFIG_SYSCLK_PSADIV SYSCLK_PSADIV_2 + #define CONFIG_SYSCLK_PSBCDIV SYSCLK_PSBCDIV_1_1 +\endcode + * + * Content of conf_clock.h for AT32UC3A0, AT32UC3A1, AT32UC3B devices (USBB): + * \code + // Configuration based on 12MHz external OSC: + #define CONFIG_PLL1_SOURCE PLL_SRC_OSC0 + #define CONFIG_PLL1_MUL 8 + #define CONFIG_PLL1_DIV 2 + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_PLL1 + #define CONFIG_USBCLK_DIV 1 // Fusb = Fsys/(2 ^ USB_div) +\endcode + * + * Content of conf_clock.h for AT32UC3A3, AT32UC3A4 devices (USBB with high speed support): + * \code + // Configuration based on 12MHz external OSC: + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_OSC0 + #define CONFIG_USBCLK_DIV 1 // Fusb = Fsys/(2 ^ USB_div) +\endcode + * + * Content of conf_clock.h for AT32UC3C, ATUCXXD, ATUCXXL3U, ATUCXXL4U devices (USBC): + * \code + // Configuration based on 12MHz external OSC: + #define CONFIG_PLL1_SOURCE PLL_SRC_OSC0 + #define CONFIG_PLL1_MUL 8 + #define CONFIG_PLL1_DIV 2 + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_PLL1 + #define CONFIG_USBCLK_DIV 1 // Fusb = Fsys/(2 ^ USB_div) + // CPU clock need of clock > 25MHz to run with USBC + #define CONFIG_SYSCLK_SOURCE SYSCLK_SRC_PLL1 +\endcode + * + * Content of conf_clock.h for SAM3S, SAM3SD, SAM4S devices (UPD: USB Peripheral Device): + * \code + // PLL1 (B) Options (Fpll = (Fclk * PLL_mul) / PLL_div) + #define CONFIG_PLL1_SOURCE PLL_SRC_MAINCK_XTAL + #define CONFIG_PLL1_MUL 16 + #define CONFIG_PLL1_DIV 2 + // USB Clock Source Options (Fusb = FpllX / USB_div) + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_PLL1 + #define CONFIG_USBCLK_DIV 2 +\endcode + * + * Content of conf_clock.h for SAM3U device (UPDHS: USB Peripheral Device High Speed): + * \code + // USB Clock Source fixed at UPLL. +\endcode + * + * Content of conf_clock.h for SAM3X, SAM3A devices (UOTGHS: USB OTG High Speed): + * \code + // USB Clock Source fixed at UPLL. + #define CONFIG_USBCLK_SOURCE USBCLK_SRC_UPLL + #define CONFIG_USBCLK_DIV 1 +\endcode + * + * Content of conf_clocks.h for SAMD devices (USB): + * \code + // System clock bus configuration + # define CONF_CLOCK_FLASH_WAIT_STATES 2 + + // USB Clock Source fixed at DFLL. + // SYSTEM_CLOCK_SOURCE_DFLL configuration - Digital Frequency Locked Loop + # define CONF_CLOCK_DFLL_ENABLE true + # define CONF_CLOCK_DFLL_LOOP_MODE SYSTEM_CLOCK_DFLL_LOOP_MODE_USB_RECOVERY + # define CONF_CLOCK_DFLL_ON_DEMAND true + + // Set this to true to configure the GCLK when running clocks_init. + // If set to false, none of the GCLK generators will be configured in clocks_init(). + # define CONF_CLOCK_CONFIGURE_GCLK true + + // Configure GCLK generator 0 (Main Clock) + # define CONF_CLOCK_GCLK_0_ENABLE true + # define CONF_CLOCK_GCLK_0_RUN_IN_STANDBY true + # define CONF_CLOCK_GCLK_0_CLOCK_SOURCE SYSTEM_CLOCK_SOURCE_DFLL + # define CONF_CLOCK_GCLK_0_PRESCALER 1 + # define CONF_CLOCK_GCLK_0_OUTPUT_ENABLE false +\endcode + */ + +/** + * \page udc_use_case_1 Change USB speed + * + * In this use case, the USB device is used with different USB speeds. + * + * \section udc_use_case_1_setup Setup steps + * + * Prior to implement this use case, be sure to have already + * apply the UDI module "basic use case". + * + * \section udc_use_case_1_usage Usage steps + * + * \subsection udc_use_case_1_usage_code Example code + * Content of conf_usb.h: + * \code + #if // Low speed + #define USB_DEVICE_LOW_SPEED + // #define USB_DEVICE_HS_SUPPORT + + #elif // Full speed + // #define USB_DEVICE_LOW_SPEED + // #define USB_DEVICE_HS_SUPPORT + + #elif // High speed + // #define USB_DEVICE_LOW_SPEED + #define USB_DEVICE_HS_SUPPORT + + #endif +\endcode + * + * \subsection udc_use_case_1_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required for a USB device low speed (1.5Mbit/s): + * - \code #define USB_DEVICE_LOW_SPEED + //#define USB_DEVICE_HS_SUPPORT \endcode + * -# Ensure that conf_usb.h contains the following parameters + * required for a USB device full speed (12Mbit/s): + * - \code //#define USB_DEVICE_LOW_SPEED + //#define USB_DEVICE_HS_SUPPORT \endcode + * -# Ensure that conf_usb.h contains the following parameters + * required for a USB device high speed (480Mbit/s): + * - \code //#define USB_DEVICE_LOW_SPEED + #define USB_DEVICE_HS_SUPPORT \endcode + */ + +/** + * \page udc_use_case_2 Use USB strings + * + * In this use case, the usual USB strings is added in the USB device. + * + * \section udc_use_case_2_setup Setup steps + * Prior to implement this use case, be sure to have already + * apply the UDI module "basic use case". + * + * \section udc_use_case_2_usage Usage steps + * + * \subsection udc_use_case_2_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_MANUFACTURE_NAME "Manufacture name" + #define USB_DEVICE_PRODUCT_NAME "Product name" + #define USB_DEVICE_SERIAL_NAME "12...EF" +\endcode + * + * \subsection udc_use_case_2_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required to enable different USB strings: + * - \code // Static ASCII name for the manufacture + #define USB_DEVICE_MANUFACTURE_NAME "Manufacture name" \endcode + * - \code // Static ASCII name for the product + #define USB_DEVICE_PRODUCT_NAME "Product name" \endcode + * - \code // Static ASCII name to enable and set a serial number + #define USB_DEVICE_SERIAL_NAME "12...EF" \endcode + */ + +/** + * \page udc_use_case_3 Use USB remote wakeup feature + * + * In this use case, the USB remote wakeup feature is enabled. + * + * \section udc_use_case_3_setup Setup steps + * Prior to implement this use case, be sure to have already + * apply the UDI module "basic use case". + * + * \section udc_use_case_3_usage Usage steps + * + * \subsection udc_use_case_3_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_ATTR \ + (USB_CONFIG_ATTR_REMOTE_WAKEUP | USB_CONFIG_ATTR_..._POWERED) + #define UDC_REMOTEWAKEUP_ENABLE() my_callback_remotewakeup_enable() + extern void my_callback_remotewakeup_enable(void); + #define UDC_REMOTEWAKEUP_DISABLE() my_callback_remotewakeup_disable() + extern void my_callback_remotewakeup_disable(void); +\endcode + * + * Add to application C-file: + * \code + void my_callback_remotewakeup_enable(void) + { + // Enable application wakeup events (e.g. enable GPIO interrupt) + } + void my_callback_remotewakeup_disable(void) + { + // Disable application wakeup events (e.g. disable GPIO interrupt) + } + + void my_interrupt_event(void) + { + udc_remotewakeup(); + } +\endcode + * + * \subsection udc_use_case_3_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required to enable remote wakeup feature: + * - \code // Authorizes the remote wakeup feature + #define USB_DEVICE_ATTR (USB_CONFIG_ATTR_REMOTE_WAKEUP | USB_CONFIG_ATTR_..._POWERED) \endcode + * - \code // Define callback called when the host enables the remotewakeup feature + #define UDC_REMOTEWAKEUP_ENABLE() my_callback_remotewakeup_enable() + extern void my_callback_remotewakeup_enable(void); \endcode + * - \code // Define callback called when the host disables the remotewakeup feature + #define UDC_REMOTEWAKEUP_DISABLE() my_callback_remotewakeup_disable() + extern void my_callback_remotewakeup_disable(void); \endcode + * -# Send a remote wakeup (USB upstream): + * - \code udc_remotewakeup(); \endcode + */ + +/** + * \page udc_use_case_5 Bus power application recommendations + * + * In this use case, the USB device BUS power feature is enabled. + * This feature requires a correct power consumption management. + * + * \section udc_use_case_5_setup Setup steps + * Prior to implement this use case, be sure to have already + * apply the UDI module "basic use case". + * + * \section udc_use_case_5_usage Usage steps + * + * \subsection udc_use_case_5_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_ATTR (USB_CONFIG_ATTR_BUS_POWERED) + #define UDC_SUSPEND_EVENT() user_callback_suspend_action() + extern void user_callback_suspend_action(void) + #define UDC_RESUME_EVENT() user_callback_resume_action() + extern void user_callback_resume_action(void) +\endcode + * + * Add to application C-file: + * \code + void user_callback_suspend_action(void) + { + // Disable hardware component to reduce power consumption + } + void user_callback_resume_action(void) + { + // Re-enable hardware component + } +\endcode + * + * \subsection udc_use_case_5_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters: + * - \code // Authorizes the BUS power feature + #define USB_DEVICE_ATTR (USB_CONFIG_ATTR_BUS_POWERED) \endcode + * - \code // Define callback called when the host suspend the USB line + #define UDC_SUSPEND_EVENT() user_callback_suspend_action() + extern void user_callback_suspend_action(void); \endcode + * - \code // Define callback called when the host or device resume the USB line + #define UDC_RESUME_EVENT() user_callback_resume_action() + extern void user_callback_resume_action(void); \endcode + * -# Reduce power consumption in suspend mode (max. 2.5mA on Vbus): + * - \code void user_callback_suspend_action(void) + { + turn_off_components(); + } \endcode + */ + +/** + * \page udc_use_case_6 USB dynamic serial number + * + * In this use case, the USB serial strings is dynamic. + * For a static serial string refer to \ref udc_use_case_2. + * + * \section udc_use_case_6_setup Setup steps + * Prior to implement this use case, be sure to have already + * apply the UDI module "basic use case". + * + * \section udc_use_case_6_usage Usage steps + * + * \subsection udc_use_case_6_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_SERIAL_NAME + #define USB_DEVICE_GET_SERIAL_NAME_POINTER serial_number + #define USB_DEVICE_GET_SERIAL_NAME_LENGTH 12 + extern uint8_t serial_number[]; +\endcode + * + * Add to application C-file: + * \code + uint8_t serial_number[USB_DEVICE_GET_SERIAL_NAME_LENGTH]; + + void init_build_usb_serial_number(void) + { + serial_number[0] = 'A'; + serial_number[1] = 'B'; + ... + serial_number[USB_DEVICE_GET_SERIAL_NAME_LENGTH-1] = 'C'; + } \endcode + * + * \subsection udc_use_case_6_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required to enable a USB serial number strings dynamically: + * - \code #define USB_DEVICE_SERIAL_NAME // Define this empty + #define USB_DEVICE_GET_SERIAL_NAME_POINTER serial_number // Give serial array pointer + #define USB_DEVICE_GET_SERIAL_NAME_LENGTH 12 // Give size of serial array + extern uint8_t serial_number[]; // Declare external serial array \endcode + * -# Before start USB stack, initialize the serial array + * - \code + uint8_t serial_number[USB_DEVICE_GET_SERIAL_NAME_LENGTH]; + + void init_build_usb_serial_number(void) + { + serial_number[0] = 'A'; + serial_number[1] = 'B'; + ... + serial_number[USB_DEVICE_GET_SERIAL_NAME_LENGTH-1] = 'C'; + } \endcode + */ + + + +#endif // _UDC_H_ diff --git a/src/HAL/DUE/usb/udc_desc.h b/src/HAL/DUE/usb/udc_desc.h new file mode 100644 index 0000000..052ca08 --- /dev/null +++ b/src/HAL/DUE/usb/udc_desc.h @@ -0,0 +1,135 @@ +/** + * \file + * + * \brief Common API for USB Device Interface + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDC_DESC_H_ +#define _UDC_DESC_H_ + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "udi.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \ingroup udc_group + * \defgroup udc_desc_group USB Device Descriptor + * + * @{ + */ + +/** + * \brief Defines the memory's location of USB descriptors + * + * By default the Descriptor is stored in RAM + * (UDC_DESC_STORAGE is defined empty). + * + * If you have need to free RAM space, + * it is possible to put descriptor in flash in following case: + * - USB driver authorize flash transfer (USBB on UC3 and USB on Mega) + * - USB Device is not high speed (UDC no need to change USB descriptors) + * + * For UC3 application used "const". + * + * For Mega application used "code". + */ +#define UDC_DESC_STORAGE + // Descriptor storage in internal RAM +#if (defined UDC_DATA_USE_HRAM_SUPPORT) +# if defined(__GNUC__) +# define UDC_DATA(x) COMPILER_WORD_ALIGNED __attribute__((__section__(".data_hram0"))) +# define UDC_BSS(x) COMPILER_ALIGNED(x) __attribute__((__section__(".bss_hram0"))) +# elif defined(__ICCAVR32__) +# define UDC_DATA(x) COMPILER_ALIGNED(x) __data32 +# define UDC_BSS(x) COMPILER_ALIGNED(x) __data32 +# endif +#else +# define UDC_DATA(x) COMPILER_ALIGNED(x) +# define UDC_BSS(x) COMPILER_ALIGNED(x) +#endif + + + +/** + * \brief Configuration descriptor and UDI link for one USB speed + */ +typedef struct { + //! USB configuration descriptor + usb_conf_desc_t UDC_DESC_STORAGE *desc; + //! Array of UDI API pointer + udi_api_t UDC_DESC_STORAGE *UDC_DESC_STORAGE * udi_apis; +} udc_config_speed_t; + + +/** + * \brief All information about the USB Device + */ +typedef struct { + //! USB device descriptor for low or full speed + usb_dev_desc_t UDC_DESC_STORAGE *confdev_lsfs; + //! USB configuration descriptor and UDI API pointers for low or full speed + udc_config_speed_t UDC_DESC_STORAGE *conf_lsfs; +#ifdef USB_DEVICE_HS_SUPPORT + //! USB device descriptor for high speed + usb_dev_desc_t UDC_DESC_STORAGE *confdev_hs; + //! USB device qualifier, only use in high speed mode + usb_dev_qual_desc_t UDC_DESC_STORAGE *qualifier; + //! USB configuration descriptor and UDI API pointers for high speed + udc_config_speed_t UDC_DESC_STORAGE *conf_hs; +#endif + usb_dev_bos_desc_t UDC_DESC_STORAGE *conf_bos; +} udc_config_t; + +//! Global variables of USB Device Descriptor and UDI links +extern UDC_DESC_STORAGE udc_config_t udc_config; + +//@} + +#ifdef __cplusplus +} +#endif +#endif // _UDC_DESC_H_ diff --git a/src/HAL/DUE/usb/udd.h b/src/HAL/DUE/usb/udd.h new file mode 100644 index 0000000..319d884 --- /dev/null +++ b/src/HAL/DUE/usb/udd.h @@ -0,0 +1,396 @@ +/** + * \file + * + * \brief Common API for USB Device Drivers (UDD) + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDD_H_ +#define _UDD_H_ + +#include "usb_protocol.h" +#include "udc_desc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \ingroup usb_device_group + * \defgroup udd_group USB Device Driver (UDD) + * + * The UDD driver provides a low-level abstraction of the device + * controller hardware. Most events coming from the hardware such as + * interrupts, which may cause the UDD to call into the UDC and UDI. + * + * @{ + */ + +//! \brief Endpoint identifier +typedef uint8_t udd_ep_id_t; + +//! \brief Endpoint transfer status +//! Returned in parameters of callback register via udd_ep_run routine. +typedef enum { + UDD_EP_TRANSFER_OK = 0, + UDD_EP_TRANSFER_ABORT = 1, +} udd_ep_status_t; + +/** + * \brief Global variable to give and record information of the setup request management + * + * This global variable allows to decode and response a setup request. + * It can be updated by udc_process_setup() from UDC or *setup() from UDIs. + */ +typedef struct { + //! Data received in USB SETUP packet + //! Note: The swap of "req.wValues" from uin16_t to le16_t is done by UDD. + usb_setup_req_t req; + + //! Point to buffer to send or fill with data following SETUP packet + //! This buffer must be word align for DATA IN phase (use prefix COMPILER_WORD_ALIGNED for buffer) + uint8_t *payload; + + //! Size of buffer to send or fill, and content the number of byte transferred + uint16_t payload_size; + + //! Callback called after reception of ZLP from setup request + void (*callback)(void); + + //! Callback called when the buffer given (.payload) is full or empty. + //! This one return false to abort data transfer, or true with a new buffer in .payload. + bool (*over_under_run)(void); +} udd_ctrl_request_t; +extern udd_ctrl_request_t udd_g_ctrlreq; + +//! Return true if the setup request \a udd_g_ctrlreq indicates IN data transfer +#define Udd_setup_is_in() \ + (USB_REQ_DIR_IN == (udd_g_ctrlreq.req.bmRequestType & USB_REQ_DIR_MASK)) + +//! Return true if the setup request \a udd_g_ctrlreq indicates OUT data transfer +#define Udd_setup_is_out() \ + (USB_REQ_DIR_OUT == (udd_g_ctrlreq.req.bmRequestType & USB_REQ_DIR_MASK)) + +//! Return the type of the SETUP request \a udd_g_ctrlreq. \see usb_reqtype. +#define Udd_setup_type() \ + (udd_g_ctrlreq.req.bmRequestType & USB_REQ_TYPE_MASK) + +//! Return the recipient of the SETUP request \a udd_g_ctrlreq. \see usb_recipient +#define Udd_setup_recipient() \ + (udd_g_ctrlreq.req.bmRequestType & USB_REQ_RECIP_MASK) + +/** + * \brief End of halt callback function type. + * Registered by routine udd_ep_wait_stall_clear() + * Callback called when endpoint stall is cleared. + */ +typedef void (*udd_callback_halt_cleared_t)(void); + +/** + * \brief End of transfer callback function type. + * Registered by routine udd_ep_run() + * Callback called by USB interrupt after data transfer or abort (reset,...). + * + * \param status UDD_EP_TRANSFER_OK, if transfer is complete + * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted + * \param n number of data transferred + */ +typedef void (*udd_callback_trans_t) (udd_ep_status_t status, + iram_size_t nb_transferred, udd_ep_id_t ep); + +/** + * \brief Authorizes the VBUS event + * + * \return true, if the VBUS monitoring is possible. + */ +bool udd_include_vbus_monitoring(void); + +/** + * \brief Enables the USB Device mode + */ +void udd_enable(void); + +/** + * \brief Disables the USB Device mode + */ +void udd_disable(void); + +/** + * \brief Attach device to the bus when possible + * + * \warning If a VBus control is included in driver, + * then it will attach device when an acceptable Vbus + * level from the host is detected. + */ +void udd_attach(void); + +/** + * \brief Detaches the device from the bus + * + * The driver must remove pull-up on USB line D- or D+. + */ +void udd_detach(void); + +/** + * \brief Test whether the USB Device Controller is running at high + * speed or not. + * + * \return \c true if the Device is running at high speed mode, otherwise \c false. + */ +bool udd_is_high_speed(void); + +/** + * \brief Changes the USB address of device + * + * \param address New USB address + */ +void udd_set_address(uint8_t address); + +/** + * \brief Returns the USB address of device + * + * \return USB address + */ +uint8_t udd_getaddress(void); + +/** + * \brief Returns the current start of frame number + * + * \return current start of frame number. + */ +uint16_t udd_get_frame_number(void); + +/** + * \brief Returns the current micro start of frame number + * + * \return current micro start of frame number required in high speed mode. + */ +uint16_t udd_get_micro_frame_number(void); + +/*! \brief The USB driver sends a resume signal called Upstream Resume + */ +void udd_send_remotewakeup(void); + +/** + * \brief Load setup payload + * + * \param payload Pointer on payload + * \param payload_size Size of payload + */ +void udd_set_setup_payload( uint8_t *payload, uint16_t payload_size ); + + +/** + * \name Endpoint Management + * + * The following functions allow drivers to create and remove + * endpoints, as well as set, clear and query their "halted" and + * "wedged" states. + */ +//@{ + +#if (USB_DEVICE_MAX_EP != 0) + +/** + * \brief Configures and enables an endpoint + * + * \param ep Endpoint number including direction (USB_EP_DIR_IN/USB_EP_DIR_OUT). + * \param bmAttributes Attributes of endpoint declared in the descriptor. + * \param MaxEndpointSize Endpoint maximum size + * + * \return \c 1 if the endpoint is enabled, otherwise \c 0. + */ +bool udd_ep_alloc(udd_ep_id_t ep, uint8_t bmAttributes, + uint16_t MaxEndpointSize); + +/** + * \brief Disables an endpoint + * + * \param ep Endpoint number including direction (USB_EP_DIR_IN/USB_EP_DIR_OUT). + */ +void udd_ep_free(udd_ep_id_t ep); + +/** + * \brief Check if the endpoint \a ep is halted. + * + * \param ep The ID of the endpoint to check. + * + * \return \c 1 if \a ep is halted, otherwise \c 0. + */ +bool udd_ep_is_halted(udd_ep_id_t ep); + +/** + * \brief Set the halted state of the endpoint \a ep + * + * After calling this function, any transaction on \a ep will result + * in a STALL handshake being sent. Any pending transactions will be + * performed first, however. + * + * \param ep The ID of the endpoint to be halted + * + * \return \c 1 if \a ep is halted, otherwise \c 0. + */ +bool udd_ep_set_halt(udd_ep_id_t ep); + +/** + * \brief Clear the halted state of the endpoint \a ep + * + * After calling this function, any transaction on \a ep will + * be handled normally, i.e. a STALL handshake will not be sent, and + * the data toggle sequence will start at DATA0. + * + * \param ep The ID of the endpoint to be un-halted + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +bool udd_ep_clear_halt(udd_ep_id_t ep); + +/** + * \brief Registers a callback to call when endpoint halt is cleared + * + * \param ep The ID of the endpoint to use + * \param callback NULL or function to call when endpoint halt is cleared + * + * \warning if the endpoint is not halted then the \a callback is called immediately. + * + * \return \c 1 if the register is accepted, otherwise \c 0. + */ +bool udd_ep_wait_stall_clear(udd_ep_id_t ep, + udd_callback_halt_cleared_t callback); + +/** + * \brief Allows to receive or send data on an endpoint + * + * The driver uses a specific DMA USB to transfer data + * from internal RAM to endpoint, if this one is available. + * When the transfer is finished or aborted (stall, reset, ...), the \a callback is called. + * The \a callback returns the transfer status and eventually the number of byte transferred. + * Note: The control endpoint is not authorized. + * + * \param ep The ID of the endpoint to use + * \param b_shortpacket Enabled automatic short packet + * \param buf Buffer on Internal RAM to send or fill. + * It must be align, then use COMPILER_WORD_ALIGNED. + * \param buf_size Buffer size to send or fill + * \param callback NULL or function to call at the end of transfer + * + * \warning About \a b_shortpacket, for IN endpoint it means that a short packet + * (or a Zero Length Packet) will be sent to the USB line to properly close the usb + * transfer at the end of the data transfer. + * For Bulk and Interrupt OUT endpoint, it will automatically stop the transfer + * at the end of the data transfer (received short packet). + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +bool udd_ep_run(udd_ep_id_t ep, bool b_shortpacket, + uint8_t * buf, iram_size_t buf_size, + udd_callback_trans_t callback); +/** + * \brief Aborts transfer on going on endpoint + * + * If a transfer is on going, then it is stopped and + * the callback registered is called to signal the end of transfer. + * Note: The control endpoint is not authorized. + * + * \param ep Endpoint to abort + */ +void udd_ep_abort(udd_ep_id_t ep); + +#endif + +//@} + + +/** + * \name High speed test mode management + * + * The following functions allow the device to jump to a specific test mode required in high speed mode. + */ +//@{ +void udd_test_mode_j(void); +void udd_test_mode_k(void); +void udd_test_mode_se0_nak(void); +void udd_test_mode_packet(void); +//@} + + +/** + * \name UDC callbacks to provide for UDD + * + * The following callbacks are used by UDD. + */ +//@{ + +/** + * \brief Decodes and manages a setup request + * + * The driver call it when a SETUP packet is received. + * The \c udd_g_ctrlreq contains the data of SETUP packet. + * If this callback accepts the setup request then it must + * return \c 1 and eventually update \c udd_g_ctrlreq to send or receive data. + * + * \return \c 1 if the request is accepted, otherwise \c 0. + */ +extern bool udc_process_setup(void); + +/** + * \brief Reset the UDC + * + * The UDC must reset all configuration. + */ +extern void udc_reset(void); + +/** + * \brief To signal that a SOF is occurred + * + * The UDC must send the signal to all UDIs enabled + */ +extern void udc_sof_notify(void); + +//@} + +//@} + +#ifdef __cplusplus +} +#endif +#endif // _UDD_H_ diff --git a/src/HAL/DUE/usb/udi.h b/src/HAL/DUE/usb/udi.h new file mode 100644 index 0000000..febf03b --- /dev/null +++ b/src/HAL/DUE/usb/udi.h @@ -0,0 +1,133 @@ +/** + * \file + * + * \brief Common API for USB Device Interface + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDI_H_ +#define _UDI_H_ + +#include "conf_usb.h" +#include "usb_protocol.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \ingroup usb_device_group + * \defgroup udi_group USB Device Interface (UDI) + * The UDI provides a common API for all classes, + * and this is used by UDC for the main control of USB Device interface. + * @{ + */ + +/** + * \brief UDI API. + * + * The callbacks within this structure are called only by + * USB Device Controller (UDC) + * + * The udc_get_interface_desc() can be use by UDI to know the interface descriptor + * selected by UDC. + */ +typedef struct { + /** + * \brief Enable the interface. + * + * This function is called when the host selects a configuration + * to which this interface belongs through a Set Configuration + * request, and when the host selects an alternate setting of + * this interface through a Set Interface request. + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ + bool (*enable)(void); + + /** + * \brief Disable the interface. + * + * This function is called when this interface is currently + * active, and + * - the host selects any configuration through a Set + * Configuration request, or + * - the host issues a USB reset, or + * - the device is detached from the host (i.e. Vbus is no + * longer present) + */ + void (*disable)(void); + + /** + * \brief Handle a control request directed at an interface. + * + * This function is called when this interface is currently + * active and the host sends a SETUP request + * with this interface as the recipient. + * + * Use udd_g_ctrlreq to decode and response to SETUP request. + * + * \return \c 1 if this interface supports the SETUP request, otherwise \c 0. + */ + bool (*setup)(void); + + /** + * \brief Returns the current setting of the selected interface. + * + * This function is called when UDC when know alternate setting of selected interface. + * + * \return alternate setting of selected interface + */ + uint8_t (*getsetting)(void); + + /** + * \brief To signal that a SOF is occurred + */ + void (*sof_notify)(void); +} udi_api_t; + +//@} + +#ifdef __cplusplus +} +#endif +#endif // _UDI_H_ diff --git a/src/HAL/DUE/usb/udi_cdc.c b/src/HAL/DUE/usb/udi_cdc.c new file mode 100644 index 0000000..89debe5 --- /dev/null +++ b/src/HAL/DUE/usb/udi_cdc.c @@ -0,0 +1,1155 @@ +/** + * \file + * + * \brief USB Device Communication Device Class (CDC) interface. + * + * Copyright (c) 2009-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "usb_protocol_cdc.h" +#include "udd.h" +#include "udc.h" +#include "udi_cdc.h" +#include + +#ifdef UDI_CDC_LOW_RATE +# ifdef USB_DEVICE_HS_SUPPORT +# define UDI_CDC_TX_BUFFERS (UDI_CDC_DATA_EPS_HS_SIZE) +# define UDI_CDC_RX_BUFFERS (UDI_CDC_DATA_EPS_HS_SIZE) +# else +# define UDI_CDC_TX_BUFFERS (UDI_CDC_DATA_EPS_FS_SIZE) +# define UDI_CDC_RX_BUFFERS (UDI_CDC_DATA_EPS_FS_SIZE) +# endif +#else +# ifdef USB_DEVICE_HS_SUPPORT +# define UDI_CDC_TX_BUFFERS (UDI_CDC_DATA_EPS_HS_SIZE) +# define UDI_CDC_RX_BUFFERS (UDI_CDC_DATA_EPS_HS_SIZE) +# else +# define UDI_CDC_TX_BUFFERS (5*UDI_CDC_DATA_EPS_FS_SIZE) +# define UDI_CDC_RX_BUFFERS (5*UDI_CDC_DATA_EPS_FS_SIZE) +# endif +#endif + +#ifndef UDI_CDC_TX_EMPTY_NOTIFY +# define UDI_CDC_TX_EMPTY_NOTIFY(port) +#endif + +/** + * \ingroup udi_cdc_group + * \defgroup udi_cdc_group_udc Interface with USB Device Core (UDC) + * + * Structures and functions required by UDC. + * + * @{ + */ +bool udi_cdc_comm_enable(void); +void udi_cdc_comm_disable(void); +bool udi_cdc_comm_setup(void); +bool udi_cdc_data_enable(void); +void udi_cdc_data_disable(void); +bool udi_cdc_data_setup(void); +uint8_t udi_cdc_getsetting(void); +void udi_cdc_data_sof_notify(void); +UDC_DESC_STORAGE udi_api_t udi_api_cdc_comm = { + .enable = udi_cdc_comm_enable, + .disable = udi_cdc_comm_disable, + .setup = udi_cdc_comm_setup, + .getsetting = udi_cdc_getsetting, +}; +UDC_DESC_STORAGE udi_api_t udi_api_cdc_data = { + .enable = udi_cdc_data_enable, + .disable = udi_cdc_data_disable, + .setup = udi_cdc_data_setup, + .getsetting = udi_cdc_getsetting, + .sof_notify = udi_cdc_data_sof_notify, +}; +//@} + +/** + * \ingroup udi_cdc_group + * \defgroup udi_cdc_group_internal Implementation of UDI CDC + * + * Class internal implementation + * @{ + */ + +/** + * \name Internal routines + */ +//@{ + +/** + * \name Routines to control serial line + */ +//@{ + +/** + * \brief Returns the port number corresponding at current setup request + * + * \return port number + */ +static uint8_t udi_cdc_setup_to_port(void); + +/** + * \brief Sends line coding to application + * + * Called after SETUP request when line coding data is received. + */ +static void udi_cdc_line_coding_received(void); + +/** + * \brief Records new state + * + * \param port Communication port number to manage + * \param b_set State is enabled if true, else disabled + * \param bit_mask Field to process (see CDC_SERIAL_STATE_ defines) + */ +static void udi_cdc_ctrl_state_change(uint8_t port, bool b_set, le16_t bit_mask); + +/** + * \brief Check and eventually notify the USB host of new state + * + * \param port Communication port number to manage + * \param ep Port communication endpoint + */ +static void udi_cdc_ctrl_state_notify(uint8_t port, udd_ep_id_t ep); + +/** + * \brief Ack sent of serial state message + * Callback called after serial state message sent + * + * \param status UDD_EP_TRANSFER_OK, if transfer finished + * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted + * \param n number of data transferred + */ +static void udi_cdc_serial_state_msg_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep); + +//@} + +/** + * \name Routines to process data transfer + */ +//@{ + +/** + * \brief Enable the reception of data from the USB host + * + * The value udi_cdc_rx_trans_sel indicate the RX buffer to fill. + * + * \param port Communication port number to manage + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +static bool udi_cdc_rx_start(uint8_t port); + +/** + * \brief Update rx buffer management with a new data + * Callback called after data reception on USB line + * + * \param status UDD_EP_TRANSFER_OK, if transfer finish + * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted + * \param n number of data received + */ +static void udi_cdc_data_received(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep); + +/** + * \brief Ack sent of tx buffer + * Callback called after data transfer on USB line + * + * \param status UDD_EP_TRANSFER_OK, if transfer finished + * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted + * \param n number of data transferred + */ +static void udi_cdc_data_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep); + +/** + * \brief Send buffer on line or wait a SOF event + * + * \param port Communication port number to manage + */ +static void udi_cdc_tx_send(uint8_t port); + +//@} + +//@} + +/** + * \name Information about configuration of communication line + */ +//@{ +COMPILER_WORD_ALIGNED + static usb_cdc_line_coding_t udi_cdc_line_coding[UDI_CDC_PORT_NB]; +static bool udi_cdc_serial_state_msg_ongoing[UDI_CDC_PORT_NB]; +static volatile le16_t udi_cdc_state[UDI_CDC_PORT_NB]; +COMPILER_WORD_ALIGNED static usb_cdc_notify_serial_state_t uid_cdc_state_msg[UDI_CDC_PORT_NB]; + +//! Status of CDC COMM interfaces +static volatile uint8_t udi_cdc_nb_comm_enabled = 0; +//@} + +/** + * \name Variables to manage RX/TX transfer requests + * Two buffers for each sense are used to optimize the speed. + */ +//@{ + +//! Status of CDC DATA interfaces +static volatile uint8_t udi_cdc_nb_data_enabled = 0; +static volatile bool udi_cdc_data_running = false; +//! Buffer to receive data +COMPILER_WORD_ALIGNED static uint8_t udi_cdc_rx_buf[UDI_CDC_PORT_NB][2][UDI_CDC_RX_BUFFERS]; +//! Data available in RX buffers +static volatile uint16_t udi_cdc_rx_buf_nb[UDI_CDC_PORT_NB][2]; +//! Give the current RX buffer used (rx0 if 0, rx1 if 1) +static volatile uint8_t udi_cdc_rx_buf_sel[UDI_CDC_PORT_NB]; +//! Read position in current RX buffer +static volatile uint16_t udi_cdc_rx_pos[UDI_CDC_PORT_NB]; +//! Signal a transfer on-going +static volatile bool udi_cdc_rx_trans_ongoing[UDI_CDC_PORT_NB]; + +//! Define a transfer halted +#define UDI_CDC_TRANS_HALTED 2 + +//! Buffer to send data +COMPILER_WORD_ALIGNED static uint8_t udi_cdc_tx_buf[UDI_CDC_PORT_NB][2][UDI_CDC_TX_BUFFERS]; +//! Data available in TX buffers +static uint16_t udi_cdc_tx_buf_nb[UDI_CDC_PORT_NB][2]; +//! Give current TX buffer used (tx0 if 0, tx1 if 1) +static volatile uint8_t udi_cdc_tx_buf_sel[UDI_CDC_PORT_NB]; +//! Value of SOF during last TX transfer +static uint16_t udi_cdc_tx_sof_num[UDI_CDC_PORT_NB]; +//! Signal a transfer on-going +static volatile bool udi_cdc_tx_trans_ongoing[UDI_CDC_PORT_NB]; +//! Signal that both buffer content data to send +static volatile bool udi_cdc_tx_both_buf_to_send[UDI_CDC_PORT_NB]; + +//@} + +bool udi_cdc_comm_enable(void) +{ + uint8_t port; + uint8_t iface_comm_num; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; + udi_cdc_nb_comm_enabled = 0; +#else + if (udi_cdc_nb_comm_enabled > UDI_CDC_PORT_NB) { + udi_cdc_nb_comm_enabled = 0; + } + port = udi_cdc_nb_comm_enabled; +#endif + + // Initialize control signal management + udi_cdc_state[port] = CPU_TO_LE16(0); + + uid_cdc_state_msg[port].header.bmRequestType = + USB_REQ_DIR_IN | USB_REQ_TYPE_CLASS | + USB_REQ_RECIP_INTERFACE; + uid_cdc_state_msg[port].header.bNotification = USB_REQ_CDC_NOTIFY_SERIAL_STATE; + uid_cdc_state_msg[port].header.wValue = LE16(0); + + switch (port) { +#define UDI_CDC_PORT_TO_IFACE_COMM(index, unused) \ + case index: \ + iface_comm_num = UDI_CDC_COMM_IFACE_NUMBER_##index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_PORT_TO_IFACE_COMM, ~) +#undef UDI_CDC_PORT_TO_IFACE_COMM + default: + iface_comm_num = UDI_CDC_COMM_IFACE_NUMBER_0; + break; + } + + uid_cdc_state_msg[port].header.wIndex = LE16(iface_comm_num); + uid_cdc_state_msg[port].header.wLength = LE16(2); + uid_cdc_state_msg[port].value = CPU_TO_LE16(0); + + udi_cdc_line_coding[port].dwDTERate = CPU_TO_LE32(UDI_CDC_DEFAULT_RATE); + udi_cdc_line_coding[port].bCharFormat = UDI_CDC_DEFAULT_STOPBITS; + udi_cdc_line_coding[port].bParityType = UDI_CDC_DEFAULT_PARITY; + udi_cdc_line_coding[port].bDataBits = UDI_CDC_DEFAULT_DATABITS; + // Call application callback + // to initialize memories or indicate that interface is enabled + UDI_CDC_SET_CODING_EXT(port,(&udi_cdc_line_coding[port])); + if (!UDI_CDC_ENABLE_EXT(port)) { + return false; + } + udi_cdc_nb_comm_enabled++; + return true; +} + +bool udi_cdc_data_enable(void) +{ + uint8_t port; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; + udi_cdc_nb_data_enabled = 0; +#else + if (udi_cdc_nb_data_enabled > UDI_CDC_PORT_NB) { + udi_cdc_nb_data_enabled = 0; + } + port = udi_cdc_nb_data_enabled; +#endif + + // Initialize TX management + udi_cdc_tx_trans_ongoing[port] = false; + udi_cdc_tx_both_buf_to_send[port] = false; + udi_cdc_tx_buf_sel[port] = 0; + udi_cdc_tx_buf_nb[port][0] = 0; + udi_cdc_tx_buf_nb[port][1] = 0; + udi_cdc_tx_sof_num[port] = 0; + udi_cdc_tx_send(port); + + // Initialize RX management + udi_cdc_rx_trans_ongoing[port] = false; + udi_cdc_rx_buf_sel[port] = 0; + udi_cdc_rx_buf_nb[port][0] = 0; + udi_cdc_rx_buf_nb[port][1] = 0; + udi_cdc_rx_pos[port] = 0; + if (!udi_cdc_rx_start(port)) { + return false; + } + udi_cdc_nb_data_enabled++; + if (udi_cdc_nb_data_enabled == UDI_CDC_PORT_NB) { + udi_cdc_data_running = true; + } + return true; +} + +void udi_cdc_comm_disable(void) +{ + Assert(udi_cdc_nb_comm_enabled != 0); + udi_cdc_nb_comm_enabled--; +} + +void udi_cdc_data_disable(void) +{ + uint8_t port; + + Assert(udi_cdc_nb_data_enabled != 0); + udi_cdc_nb_data_enabled--; + port = udi_cdc_nb_data_enabled; + UDI_CDC_DISABLE_EXT(port); + udi_cdc_data_running = false; +} + +bool udi_cdc_comm_setup(void) +{ + uint8_t port = udi_cdc_setup_to_port(); + + if (Udd_setup_is_in()) { + // GET Interface Requests + if (Udd_setup_type() == USB_REQ_TYPE_CLASS) { + // Requests Class Interface Get + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_CDC_GET_LINE_CODING: + // Get configuration of CDC line + if (sizeof(usb_cdc_line_coding_t) != + udd_g_ctrlreq.req.wLength) + return false; // Error for USB host + udd_g_ctrlreq.payload = + (uint8_t *) & + udi_cdc_line_coding[port]; + udd_g_ctrlreq.payload_size = + sizeof(usb_cdc_line_coding_t); + return true; + } + } + } + if (Udd_setup_is_out()) { + // SET Interface Requests + if (Udd_setup_type() == USB_REQ_TYPE_CLASS) { + // Requests Class Interface Set + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_CDC_SET_LINE_CODING: + // Change configuration of CDC line + if (sizeof(usb_cdc_line_coding_t) != + udd_g_ctrlreq.req.wLength) + return false; // Error for USB host + udd_g_ctrlreq.callback = + udi_cdc_line_coding_received; + udd_g_ctrlreq.payload = + (uint8_t *) & + udi_cdc_line_coding[port]; + udd_g_ctrlreq.payload_size = + sizeof(usb_cdc_line_coding_t); + return true; + case USB_REQ_CDC_SET_CONTROL_LINE_STATE: + // According cdc spec 1.1 chapter 6.2.14 + UDI_CDC_SET_DTR_EXT(port, (0 != + (udd_g_ctrlreq.req.wValue + & CDC_CTRL_SIGNAL_DTE_PRESENT))); + UDI_CDC_SET_RTS_EXT(port, (0 != + (udd_g_ctrlreq.req.wValue + & CDC_CTRL_SIGNAL_ACTIVATE_CARRIER))); + return true; + } + } + } + return false; // request Not supported +} + +bool udi_cdc_data_setup(void) +{ + return false; // request Not supported +} + +uint8_t udi_cdc_getsetting(void) +{ + return 0; // CDC don't have multiple alternate setting +} + +void udi_cdc_data_sof_notify(void) +{ + static uint8_t port_notify = 0; + + // A call of udi_cdc_data_sof_notify() is done for each port + udi_cdc_tx_send(port_notify); +#if UDI_CDC_PORT_NB != 1 // To optimize code + port_notify++; + if (port_notify >= UDI_CDC_PORT_NB) { + port_notify = 0; + } +#endif +} + + +// ------------------------ +//------- Internal routines to control serial line + +static uint8_t udi_cdc_setup_to_port(void) +{ + uint8_t port; + + switch (udd_g_ctrlreq.req.wIndex & 0xFF) { +#define UDI_CDC_IFACE_COMM_TO_PORT(iface, unused) \ + case UDI_CDC_COMM_IFACE_NUMBER_##iface: \ + port = iface; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_IFACE_COMM_TO_PORT, ~) +#undef UDI_CDC_IFACE_COMM_TO_PORT + default: + port = 0; + break; + } + return port; +} + +static void udi_cdc_line_coding_received(void) +{ + uint8_t port = udi_cdc_setup_to_port(); + UNUSED(port); + + UDI_CDC_SET_CODING_EXT(port, (&udi_cdc_line_coding[port])); +} + +static void udi_cdc_ctrl_state_change(uint8_t port, bool b_set, le16_t bit_mask) +{ + irqflags_t flags; + udd_ep_id_t ep_comm; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + // Update state + flags = cpu_irq_save(); // Protect udi_cdc_state + if (b_set) { + udi_cdc_state[port] |= bit_mask; + } else { + udi_cdc_state[port] &= ~(unsigned)bit_mask; + } + cpu_irq_restore(flags); + + // Send it if possible and state changed + switch (port) { +#define UDI_CDC_PORT_TO_COMM_EP(index, unused) \ + case index: \ + ep_comm = UDI_CDC_COMM_EP_##index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_PORT_TO_COMM_EP, ~) +#undef UDI_CDC_PORT_TO_COMM_EP + default: + ep_comm = UDI_CDC_COMM_EP_0; + break; + } + udi_cdc_ctrl_state_notify(port, ep_comm); +} + + +static void udi_cdc_ctrl_state_notify(uint8_t port, udd_ep_id_t ep) +{ +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + // Send it if possible and state changed + if ((!udi_cdc_serial_state_msg_ongoing[port]) + && (udi_cdc_state[port] != uid_cdc_state_msg[port].value)) { + // Fill notification message + uid_cdc_state_msg[port].value = udi_cdc_state[port]; + // Send notification message + udi_cdc_serial_state_msg_ongoing[port] = + udd_ep_run(ep, + false, + (uint8_t *) & uid_cdc_state_msg[port], + sizeof(uid_cdc_state_msg[0]), + udi_cdc_serial_state_msg_sent); + } +} + + +static void udi_cdc_serial_state_msg_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep) +{ + uint8_t port; + UNUSED(n); + UNUSED(status); + + switch (ep) { +#define UDI_CDC_GET_PORT_FROM_COMM_EP(iface, unused) \ + case UDI_CDC_COMM_EP_##iface: \ + port = iface; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_GET_PORT_FROM_COMM_EP, ~) +#undef UDI_CDC_GET_PORT_FROM_COMM_EP + default: + port = 0; + break; + } + + udi_cdc_serial_state_msg_ongoing[port] = false; + + // For the irregular signals like break, the incoming ring signal, + // or the overrun error state, this will reset their values to zero + // and again will not send another notification until their state changes. + udi_cdc_state[port] &= ~(CDC_SERIAL_STATE_BREAK | + CDC_SERIAL_STATE_RING | + CDC_SERIAL_STATE_FRAMING | + CDC_SERIAL_STATE_PARITY | CDC_SERIAL_STATE_OVERRUN); + uid_cdc_state_msg[port].value &= ~(CDC_SERIAL_STATE_BREAK | + CDC_SERIAL_STATE_RING | + CDC_SERIAL_STATE_FRAMING | + CDC_SERIAL_STATE_PARITY | CDC_SERIAL_STATE_OVERRUN); + // Send it if possible and state changed + udi_cdc_ctrl_state_notify(port, ep); +} + + +// ------------------------ +//------- Internal routines to process data transfer + + +static bool udi_cdc_rx_start(uint8_t port) +{ + irqflags_t flags; + uint8_t buf_sel_trans; + udd_ep_id_t ep; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + flags = cpu_irq_save(); + buf_sel_trans = udi_cdc_rx_buf_sel[port]; + if (udi_cdc_rx_trans_ongoing[port] || + (udi_cdc_rx_pos[port] < udi_cdc_rx_buf_nb[port][buf_sel_trans])) { + // Transfer already on-going or current buffer no empty + cpu_irq_restore(flags); + return false; + } + + // Change current buffer + udi_cdc_rx_pos[port] = 0; + udi_cdc_rx_buf_sel[port] = (buf_sel_trans==0)?1:0; + + // Start transfer on RX + udi_cdc_rx_trans_ongoing[port] = true; + cpu_irq_restore(flags); + + if (udi_cdc_multi_is_rx_ready(port)) { + UDI_CDC_RX_NOTIFY(port); + } + // Send the buffer with enable of short packet + switch (port) { +#define UDI_CDC_PORT_TO_DATA_EP_OUT(index, unused) \ + case index: \ + ep = UDI_CDC_DATA_EP_OUT_##index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_PORT_TO_DATA_EP_OUT, ~) +#undef UDI_CDC_PORT_TO_DATA_EP_OUT + default: + ep = UDI_CDC_DATA_EP_OUT_0; + break; + } + return udd_ep_run(ep, + true, + udi_cdc_rx_buf[port][buf_sel_trans], + UDI_CDC_RX_BUFFERS, + udi_cdc_data_received); +} + + +static void udi_cdc_data_received(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep) +{ + uint8_t buf_sel_trans; + uint8_t port; + + switch (ep) { +#define UDI_CDC_DATA_EP_OUT_TO_PORT(index, unused) \ + case UDI_CDC_DATA_EP_OUT_##index: \ + port = index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_DATA_EP_OUT_TO_PORT, ~) +#undef UDI_CDC_DATA_EP_OUT_TO_PORT + default: + port = 0; + break; + } + + if (UDD_EP_TRANSFER_OK != status) { + // Abort reception + return; + } + buf_sel_trans = (udi_cdc_rx_buf_sel[port]==0)?1:0; + if (!n) { + udd_ep_run( ep, + true, + udi_cdc_rx_buf[port][buf_sel_trans], + UDI_CDC_RX_BUFFERS, + udi_cdc_data_received); + return; + } + udi_cdc_rx_buf_nb[port][buf_sel_trans] = n; + udi_cdc_rx_trans_ongoing[port] = false; + udi_cdc_rx_start(port); +} + + +static void udi_cdc_data_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep) +{ + uint8_t port; + UNUSED(n); + + switch (ep) { +#define UDI_CDC_DATA_EP_IN_TO_PORT(index, unused) \ + case UDI_CDC_DATA_EP_IN_##index: \ + port = index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_DATA_EP_IN_TO_PORT, ~) +#undef UDI_CDC_DATA_EP_IN_TO_PORT + default: + port = 0; + break; + } + + if (UDD_EP_TRANSFER_OK != status) { + // Abort transfer + return; + } + udi_cdc_tx_buf_nb[port][(udi_cdc_tx_buf_sel[port]==0)?1:0] = 0; + udi_cdc_tx_both_buf_to_send[port] = false; + udi_cdc_tx_trans_ongoing[port] = false; + + if (n != 0) { + UDI_CDC_TX_EMPTY_NOTIFY(port); + } + udi_cdc_tx_send(port); +} + + +static void udi_cdc_tx_send(uint8_t port) +{ + irqflags_t flags; + uint8_t buf_sel_trans; + bool b_short_packet; + udd_ep_id_t ep; + static uint16_t sof_zlp_counter = 0; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + if (udi_cdc_tx_trans_ongoing[port]) { + return; // Already on going or wait next SOF to send next data + } + if (udd_is_high_speed()) { + if (udi_cdc_tx_sof_num[port] == udd_get_micro_frame_number()) { + return; // Wait next SOF to send next data + } + }else{ + if (udi_cdc_tx_sof_num[port] == udd_get_frame_number()) { + return; // Wait next SOF to send next data + } + } + + flags = cpu_irq_save(); // to protect udi_cdc_tx_buf_sel + buf_sel_trans = udi_cdc_tx_buf_sel[port]; + if (udi_cdc_tx_buf_nb[port][buf_sel_trans] == 0) { + sof_zlp_counter++; + if (((!udd_is_high_speed()) && (sof_zlp_counter < 100)) + || (udd_is_high_speed() && (sof_zlp_counter < 800))) { + cpu_irq_restore(flags); + return; + } + } + sof_zlp_counter = 0; + + if (!udi_cdc_tx_both_buf_to_send[port]) { + // Send current Buffer + // and switch the current buffer + udi_cdc_tx_buf_sel[port] = (buf_sel_trans==0)?1:0; + }else{ + // Send the other Buffer + // and no switch the current buffer + buf_sel_trans = (buf_sel_trans==0)?1:0; + } + udi_cdc_tx_trans_ongoing[port] = true; + cpu_irq_restore(flags); + + b_short_packet = (udi_cdc_tx_buf_nb[port][buf_sel_trans] != UDI_CDC_TX_BUFFERS); + if (b_short_packet) { + if (udd_is_high_speed()) { + udi_cdc_tx_sof_num[port] = udd_get_micro_frame_number(); + }else{ + udi_cdc_tx_sof_num[port] = udd_get_frame_number(); + } + }else{ + udi_cdc_tx_sof_num[port] = 0; // Force next transfer without wait SOF + } + + // Send the buffer with enable of short packet + switch (port) { +#define UDI_CDC_PORT_TO_DATA_EP_IN(index, unused) \ + case index: \ + ep = UDI_CDC_DATA_EP_IN_##index; \ + break; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_PORT_TO_DATA_EP_IN, ~) +#undef UDI_CDC_PORT_TO_DATA_EP_IN + default: + ep = UDI_CDC_DATA_EP_IN_0; + break; + } + udd_ep_run( ep, + b_short_packet, + udi_cdc_tx_buf[port][buf_sel_trans], + udi_cdc_tx_buf_nb[port][buf_sel_trans], + udi_cdc_data_sent); +} + + +// ------------------------ +//------- Application interface + + +//------- Application interface + +void udi_cdc_ctrl_signal_dcd(bool b_set) +{ + udi_cdc_ctrl_state_change(0, b_set, CDC_SERIAL_STATE_DCD); +} + +void udi_cdc_ctrl_signal_dsr(bool b_set) +{ + udi_cdc_ctrl_state_change(0, b_set, CDC_SERIAL_STATE_DSR); +} + +void udi_cdc_signal_framing_error(void) +{ + udi_cdc_ctrl_state_change(0, true, CDC_SERIAL_STATE_FRAMING); +} + +void udi_cdc_signal_parity_error(void) +{ + udi_cdc_ctrl_state_change(0, true, CDC_SERIAL_STATE_PARITY); +} + +void udi_cdc_signal_overrun(void) +{ + udi_cdc_ctrl_state_change(0, true, CDC_SERIAL_STATE_OVERRUN); +} + +void udi_cdc_multi_ctrl_signal_dcd(uint8_t port, bool b_set) +{ + udi_cdc_ctrl_state_change(port, b_set, CDC_SERIAL_STATE_DCD); +} + +void udi_cdc_multi_ctrl_signal_dsr(uint8_t port, bool b_set) +{ + udi_cdc_ctrl_state_change(port, b_set, CDC_SERIAL_STATE_DSR); +} + +void udi_cdc_multi_signal_framing_error(uint8_t port) +{ + udi_cdc_ctrl_state_change(port, true, CDC_SERIAL_STATE_FRAMING); +} + +void udi_cdc_multi_signal_parity_error(uint8_t port) +{ + udi_cdc_ctrl_state_change(port, true, CDC_SERIAL_STATE_PARITY); +} + +void udi_cdc_multi_signal_overrun(uint8_t port) +{ + udi_cdc_ctrl_state_change(port, true, CDC_SERIAL_STATE_OVERRUN); +} + +iram_size_t udi_cdc_multi_get_nb_received_data(uint8_t port) +{ + irqflags_t flags; + uint16_t pos; + iram_size_t nb_received; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + flags = cpu_irq_save(); + pos = udi_cdc_rx_pos[port]; + nb_received = udi_cdc_rx_buf_nb[port][udi_cdc_rx_buf_sel[port]] - pos; + cpu_irq_restore(flags); + return nb_received; +} + +iram_size_t udi_cdc_get_nb_received_data(void) +{ + return udi_cdc_multi_get_nb_received_data(0); +} + +bool udi_cdc_multi_is_rx_ready(uint8_t port) +{ + return (udi_cdc_multi_get_nb_received_data(port) > 0); +} + +bool udi_cdc_is_rx_ready(void) +{ + return udi_cdc_multi_is_rx_ready(0); +} + +int udi_cdc_multi_getc(uint8_t port) +{ + irqflags_t flags; + int rx_data = 0; + bool b_databit_9; + uint16_t pos; + uint8_t buf_sel; + bool again; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + b_databit_9 = (9 == udi_cdc_line_coding[port].bDataBits); + +udi_cdc_getc_process_one_byte: + // Check available data + flags = cpu_irq_save(); + pos = udi_cdc_rx_pos[port]; + buf_sel = udi_cdc_rx_buf_sel[port]; + again = pos >= udi_cdc_rx_buf_nb[port][buf_sel]; + cpu_irq_restore(flags); + while (again) { + if (!udi_cdc_data_running) { + return 0; + } + goto udi_cdc_getc_process_one_byte; + } + + // Read data + rx_data |= udi_cdc_rx_buf[port][buf_sel][pos]; + udi_cdc_rx_pos[port] = pos+1; + + udi_cdc_rx_start(port); + + if (b_databit_9) { + // Receive MSB + b_databit_9 = false; + rx_data = rx_data << 8; + goto udi_cdc_getc_process_one_byte; + } + return rx_data; +} + +int udi_cdc_getc(void) +{ + return udi_cdc_multi_getc(0); +} + +iram_size_t udi_cdc_multi_read_buf(uint8_t port, void* buf, iram_size_t size) +{ + irqflags_t flags; + uint8_t *ptr_buf = (uint8_t *)buf; + iram_size_t copy_nb; + uint16_t pos; + uint8_t buf_sel; + bool again; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + +udi_cdc_read_buf_loop_wait: + // Check available data + flags = cpu_irq_save(); + pos = udi_cdc_rx_pos[port]; + buf_sel = udi_cdc_rx_buf_sel[port]; + again = pos >= udi_cdc_rx_buf_nb[port][buf_sel]; + cpu_irq_restore(flags); + while (again) { + if (!udi_cdc_data_running) { + return size; + } + goto udi_cdc_read_buf_loop_wait; + } + + // Read data + copy_nb = udi_cdc_rx_buf_nb[port][buf_sel] - pos; + if (copy_nb>size) { + copy_nb = size; + } + memcpy(ptr_buf, &udi_cdc_rx_buf[port][buf_sel][pos], copy_nb); + udi_cdc_rx_pos[port] += copy_nb; + ptr_buf += copy_nb; + size -= copy_nb; + udi_cdc_rx_start(port); + + if (size) { + goto udi_cdc_read_buf_loop_wait; + } + return 0; +} + +static iram_size_t udi_cdc_multi_read_no_polling(uint8_t port, void* buf, iram_size_t size) +{ + uint8_t *ptr_buf = (uint8_t *)buf; + iram_size_t nb_avail_data; + uint16_t pos; + uint8_t buf_sel; + irqflags_t flags; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + //Data interface not started... exit + if (!udi_cdc_data_running) { + return 0; + } + + //Get number of available data + // Check available data + flags = cpu_irq_save(); // to protect udi_cdc_rx_pos & udi_cdc_rx_buf_sel + pos = udi_cdc_rx_pos[port]; + buf_sel = udi_cdc_rx_buf_sel[port]; + nb_avail_data = udi_cdc_rx_buf_nb[port][buf_sel] - pos; + cpu_irq_restore(flags); + //If the buffer contains less than the requested number of data, + //adjust read size + if(nb_avail_data0) { + memcpy(ptr_buf, &udi_cdc_rx_buf[port][buf_sel][pos], size); + flags = cpu_irq_save(); // to protect udi_cdc_rx_pos + udi_cdc_rx_pos[port] += size; + cpu_irq_restore(flags); + + ptr_buf += size; + udi_cdc_rx_start(port); + } + return(nb_avail_data); +} + +iram_size_t udi_cdc_read_no_polling(void* buf, iram_size_t size) +{ + return udi_cdc_multi_read_no_polling(0, buf, size); +} + +iram_size_t udi_cdc_read_buf(void* buf, iram_size_t size) +{ + return udi_cdc_multi_read_buf(0, buf, size); +} + +iram_size_t __attribute__((optimize("O0"))) udi_cdc_multi_get_free_tx_buffer(uint8_t port) +{ + irqflags_t flags; + iram_size_t buf_sel_nb, retval; + uint8_t buf_sel; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + flags = cpu_irq_save(); + buf_sel = udi_cdc_tx_buf_sel[port]; + buf_sel_nb = udi_cdc_tx_buf_nb[port][buf_sel]; + if (buf_sel_nb == UDI_CDC_TX_BUFFERS) { + if ((!udi_cdc_tx_trans_ongoing[port]) + && (!udi_cdc_tx_both_buf_to_send[port])) { + /* One buffer is full, but the other buffer is not used. + * (not used = transfer on-going) + * then move to the other buffer to store data */ + udi_cdc_tx_both_buf_to_send[port] = true; + udi_cdc_tx_buf_sel[port] = (buf_sel == 0)? 1 : 0; + buf_sel_nb = 0; + } + } + retval = UDI_CDC_TX_BUFFERS - buf_sel_nb; + cpu_irq_restore(flags); + return retval; +} + +iram_size_t udi_cdc_get_free_tx_buffer(void) +{ + return udi_cdc_multi_get_free_tx_buffer(0); +} + +bool udi_cdc_multi_is_tx_ready(uint8_t port) +{ + return (udi_cdc_multi_get_free_tx_buffer(port) != 0); +} + +bool udi_cdc_is_tx_ready(void) +{ + return udi_cdc_multi_is_tx_ready(0); +} + +int udi_cdc_multi_putc(uint8_t port, int value) +{ + irqflags_t flags; + bool b_databit_9; + uint8_t buf_sel; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + b_databit_9 = (9 == udi_cdc_line_coding[port].bDataBits); + +udi_cdc_putc_process_one_byte: + // Check available space + if (!udi_cdc_multi_is_tx_ready(port)) { + if (!udi_cdc_data_running) { + return false; + } + goto udi_cdc_putc_process_one_byte; + } + + // Write value + flags = cpu_irq_save(); + buf_sel = udi_cdc_tx_buf_sel[port]; + udi_cdc_tx_buf[port][buf_sel][udi_cdc_tx_buf_nb[port][buf_sel]++] = value; + cpu_irq_restore(flags); + + if (b_databit_9) { + // Send MSB + b_databit_9 = false; + value = value >> 8; + goto udi_cdc_putc_process_one_byte; + } + return true; +} + +int udi_cdc_putc(int value) +{ + return udi_cdc_multi_putc(0, value); +} + +iram_size_t __attribute__((optimize("O0"))) udi_cdc_multi_write_buf(uint8_t port, const void* buf, iram_size_t size) +{ + irqflags_t flags; + uint8_t buf_sel; + uint16_t buf_nb; + iram_size_t copy_nb; + uint8_t *ptr_buf = (uint8_t *)buf; + +#if UDI_CDC_PORT_NB == 1 // To optimize code + port = 0; +#endif + + if (9 == udi_cdc_line_coding[port].bDataBits) { + size *=2; + } + +udi_cdc_write_buf_loop_wait: + // Check available space + if (!udi_cdc_multi_is_tx_ready(port)) { + if (!udi_cdc_data_running) { + return size; + } + goto udi_cdc_write_buf_loop_wait; + } + + // Write values + flags = cpu_irq_save(); + buf_sel = udi_cdc_tx_buf_sel[port]; + buf_nb = udi_cdc_tx_buf_nb[port][buf_sel]; + copy_nb = UDI_CDC_TX_BUFFERS - buf_nb; + if (copy_nb > size) { + copy_nb = size; + } + memcpy(&udi_cdc_tx_buf[port][buf_sel][buf_nb], ptr_buf, copy_nb); + udi_cdc_tx_buf_nb[port][buf_sel] = buf_nb + copy_nb; + cpu_irq_restore(flags); + + // Update buffer pointer + ptr_buf = ptr_buf + copy_nb; + size -= copy_nb; + + if (size) { + goto udi_cdc_write_buf_loop_wait; + } + + return 0; +} + +iram_size_t udi_cdc_write_buf(const void* buf, iram_size_t size) +{ + return udi_cdc_multi_write_buf(0, buf, size); +} + +//@} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/udi_cdc.h b/src/HAL/DUE/usb/udi_cdc.h new file mode 100644 index 0000000..b618450 --- /dev/null +++ b/src/HAL/DUE/usb/udi_cdc.h @@ -0,0 +1,810 @@ +/** + * \file + * + * \brief USB Device Communication Device Class (CDC) interface definitions. + * + * Copyright (c) 2009-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDI_CDC_H_ +#define _UDI_CDC_H_ + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "usb_protocol_cdc.h" +#include "udd.h" +#include "udc_desc.h" +#include "udi.h" + +// Check the number of port +#ifndef UDI_CDC_PORT_NB +# define UDI_CDC_PORT_NB 1 +#endif +#if (UDI_CDC_PORT_NB < 1) || (UDI_CDC_PORT_NB > 7) +# error UDI_CDC_PORT_NB must be between 1 and 7 +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \addtogroup udi_cdc_group_udc + * @{ + */ + +//! Global structure which contains standard UDI API for UDC +extern UDC_DESC_STORAGE udi_api_t udi_api_cdc_comm; +extern UDC_DESC_STORAGE udi_api_t udi_api_cdc_data; +//@} + +/** + * \ingroup udi_cdc_group + * \defgroup udi_cdc_group_desc USB interface descriptors + * + * The following structures provide predefined USB interface descriptors. + * It must be used to define the final USB descriptors. + */ +//@{ + +/** + * \brief Communication Class interface descriptor + * + * Interface descriptor with associated functional and endpoint + * descriptors for the CDC Communication Class interface. + */ +typedef struct { + //! Standard interface descriptor + usb_iface_desc_t iface; + //! CDC Header functional descriptor + usb_cdc_hdr_desc_t header; + //! CDC Abstract Control Model functional descriptor + usb_cdc_acm_desc_t acm; + //! CDC Union functional descriptor + usb_cdc_union_desc_t union_desc; + //! CDC Call Management functional descriptor + usb_cdc_call_mgmt_desc_t call_mgmt; + //! Notification endpoint descriptor + usb_ep_desc_t ep_notify; +} udi_cdc_comm_desc_t; + + +/** + * \brief Data Class interface descriptor + * + * Interface descriptor with associated endpoint descriptors for the + * CDC Data Class interface. + */ +typedef struct { + //! Standard interface descriptor + usb_iface_desc_t iface; + //! Data IN/OUT endpoint descriptors + usb_ep_desc_t ep_in; + usb_ep_desc_t ep_out; +} udi_cdc_data_desc_t; + + +//! CDC communication endpoints size for all speeds +#define UDI_CDC_COMM_EP_SIZE 64 +//! CDC data endpoints size for FS speed (8B, 16B, 32B, 64B) +#define UDI_CDC_DATA_EPS_FS_SIZE 64 +//! CDC data endpoints size for HS speed (512B only) +#define UDI_CDC_DATA_EPS_HS_SIZE 512 + +/** + * \name Content of interface descriptors + * Up to 7 CDC interfaces can be implemented on a USB device. + */ +//@{ +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_0 +#define UDI_CDC_IAD_STRING_ID_0 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_0 +#define UDI_CDC_COMM_STRING_ID_0 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_0 +#define UDI_CDC_DATA_STRING_ID_0 0 +#endif +#define UDI_CDC_IAD_DESC_0 UDI_CDC_IAD_DESC(0) +#define UDI_CDC_COMM_DESC_0 UDI_CDC_COMM_DESC(0) +#define UDI_CDC_DATA_DESC_0_FS UDI_CDC_DATA_DESC_FS(0) +#define UDI_CDC_DATA_DESC_0_HS UDI_CDC_DATA_DESC_HS(0) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_1 +#define UDI_CDC_IAD_STRING_ID_1 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_1 +#define UDI_CDC_COMM_STRING_ID_1 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_1 +#define UDI_CDC_DATA_STRING_ID_1 0 +#endif +#define UDI_CDC_IAD_DESC_1 UDI_CDC_IAD_DESC(1) +#define UDI_CDC_COMM_DESC_1 UDI_CDC_COMM_DESC(1) +#define UDI_CDC_DATA_DESC_1_FS UDI_CDC_DATA_DESC_FS(1) +#define UDI_CDC_DATA_DESC_1_HS UDI_CDC_DATA_DESC_HS(1) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_2 +#define UDI_CDC_IAD_STRING_ID_2 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_2 +#define UDI_CDC_COMM_STRING_ID_2 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_2 +#define UDI_CDC_DATA_STRING_ID_2 0 +#endif +#define UDI_CDC_IAD_DESC_2 UDI_CDC_IAD_DESC(2) +#define UDI_CDC_COMM_DESC_2 UDI_CDC_COMM_DESC(2) +#define UDI_CDC_DATA_DESC_2_FS UDI_CDC_DATA_DESC_FS(2) +#define UDI_CDC_DATA_DESC_2_HS UDI_CDC_DATA_DESC_HS(2) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_3 +#define UDI_CDC_IAD_STRING_ID_3 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_3 +#define UDI_CDC_COMM_STRING_ID_3 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_3 +#define UDI_CDC_DATA_STRING_ID_3 0 +#endif +#define UDI_CDC_IAD_DESC_3 UDI_CDC_IAD_DESC(3) +#define UDI_CDC_COMM_DESC_3 UDI_CDC_COMM_DESC(3) +#define UDI_CDC_DATA_DESC_3_FS UDI_CDC_DATA_DESC_FS(3) +#define UDI_CDC_DATA_DESC_3_HS UDI_CDC_DATA_DESC_HS(3) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_4 +#define UDI_CDC_IAD_STRING_ID_4 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_4 +#define UDI_CDC_COMM_STRING_ID_4 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_4 +#define UDI_CDC_DATA_STRING_ID_4 0 +#endif +#define UDI_CDC_IAD_DESC_4 UDI_CDC_IAD_DESC(4) +#define UDI_CDC_COMM_DESC_4 UDI_CDC_COMM_DESC(4) +#define UDI_CDC_DATA_DESC_4_FS UDI_CDC_DATA_DESC_FS(4) +#define UDI_CDC_DATA_DESC_4_HS UDI_CDC_DATA_DESC_HS(4) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_5 +#define UDI_CDC_IAD_STRING_ID_5 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_5 +#define UDI_CDC_COMM_STRING_ID_5 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_5 +#define UDI_CDC_DATA_STRING_ID_5 0 +#endif +#define UDI_CDC_IAD_DESC_5 UDI_CDC_IAD_DESC(5) +#define UDI_CDC_COMM_DESC_5 UDI_CDC_COMM_DESC(5) +#define UDI_CDC_DATA_DESC_5_FS UDI_CDC_DATA_DESC_FS(5) +#define UDI_CDC_DATA_DESC_5_HS UDI_CDC_DATA_DESC_HS(5) + +//! By default no string associated to these interfaces +#ifndef UDI_CDC_IAD_STRING_ID_6 +#define UDI_CDC_IAD_STRING_ID_6 0 +#endif +#ifndef UDI_CDC_COMM_STRING_ID_6 +#define UDI_CDC_COMM_STRING_ID_6 0 +#endif +#ifndef UDI_CDC_DATA_STRING_ID_6 +#define UDI_CDC_DATA_STRING_ID_6 0 +#endif +#define UDI_CDC_IAD_DESC_6 UDI_CDC_IAD_DESC(6) +#define UDI_CDC_COMM_DESC_6 UDI_CDC_COMM_DESC(6) +#define UDI_CDC_DATA_DESC_6_FS UDI_CDC_DATA_DESC_FS(6) +#define UDI_CDC_DATA_DESC_6_HS UDI_CDC_DATA_DESC_HS(6) +//@} + + +//! Content of CDC IAD interface descriptor for all speeds +#define UDI_CDC_IAD_DESC(port) { \ + .bLength = sizeof(usb_iad_desc_t),\ + .bDescriptorType = USB_DT_IAD,\ + .bInterfaceCount = 2,\ + .bFunctionClass = CDC_CLASS_COMM,\ + .bFunctionSubClass = CDC_SUBCLASS_ACM,\ + .bFunctionProtocol = CDC_PROTOCOL_V25TER,\ + .bFirstInterface = UDI_CDC_COMM_IFACE_NUMBER_##port,\ + .iFunction = UDI_CDC_IAD_STRING_ID_##port,\ + } + +//! Content of CDC COMM interface descriptor for all speeds +#define UDI_CDC_COMM_DESC(port) { \ + .iface.bLength = sizeof(usb_iface_desc_t),\ + .iface.bDescriptorType = USB_DT_INTERFACE,\ + .iface.bAlternateSetting = 0,\ + .iface.bNumEndpoints = 1,\ + .iface.bInterfaceClass = CDC_CLASS_COMM,\ + .iface.bInterfaceSubClass = CDC_SUBCLASS_ACM,\ + .iface.bInterfaceProtocol = CDC_PROTOCOL_V25TER,\ + .header.bFunctionLength = sizeof(usb_cdc_hdr_desc_t),\ + .header.bDescriptorType = CDC_CS_INTERFACE,\ + .header.bDescriptorSubtype = CDC_SCS_HEADER,\ + .header.bcdCDC = LE16(0x0110),\ + .call_mgmt.bFunctionLength = sizeof(usb_cdc_call_mgmt_desc_t),\ + .call_mgmt.bDescriptorType = CDC_CS_INTERFACE,\ + .call_mgmt.bDescriptorSubtype = CDC_SCS_CALL_MGMT,\ + .call_mgmt.bmCapabilities = \ + CDC_CALL_MGMT_SUPPORTED | CDC_CALL_MGMT_OVER_DCI,\ + .acm.bFunctionLength = sizeof(usb_cdc_acm_desc_t),\ + .acm.bDescriptorType = CDC_CS_INTERFACE,\ + .acm.bDescriptorSubtype = CDC_SCS_ACM,\ + .acm.bmCapabilities = CDC_ACM_SUPPORT_LINE_REQUESTS,\ + .union_desc.bFunctionLength = sizeof(usb_cdc_union_desc_t),\ + .union_desc.bDescriptorType = CDC_CS_INTERFACE,\ + .union_desc.bDescriptorSubtype= CDC_SCS_UNION,\ + .ep_notify.bLength = sizeof(usb_ep_desc_t),\ + .ep_notify.bDescriptorType = USB_DT_ENDPOINT,\ + .ep_notify.bmAttributes = USB_EP_TYPE_INTERRUPT,\ + .ep_notify.wMaxPacketSize = LE16(UDI_CDC_COMM_EP_SIZE),\ + .ep_notify.bInterval = 0x10,\ + .ep_notify.bEndpointAddress = UDI_CDC_COMM_EP_##port,\ + .iface.bInterfaceNumber = UDI_CDC_COMM_IFACE_NUMBER_##port,\ + .call_mgmt.bDataInterface = UDI_CDC_DATA_IFACE_NUMBER_##port,\ + .union_desc.bMasterInterface = UDI_CDC_COMM_IFACE_NUMBER_##port,\ + .union_desc.bSlaveInterface0 = UDI_CDC_DATA_IFACE_NUMBER_##port,\ + .iface.iInterface = UDI_CDC_COMM_STRING_ID_##port,\ + } + +//! Content of CDC DATA interface descriptors +#define UDI_CDC_DATA_DESC_COMMON \ + .iface.bLength = sizeof(usb_iface_desc_t),\ + .iface.bDescriptorType = USB_DT_INTERFACE,\ + .iface.bAlternateSetting = 0,\ + .iface.bNumEndpoints = 2,\ + .iface.bInterfaceClass = CDC_CLASS_DATA,\ + .iface.bInterfaceSubClass = 0,\ + .iface.bInterfaceProtocol = 0,\ + .ep_in.bLength = sizeof(usb_ep_desc_t),\ + .ep_in.bDescriptorType = USB_DT_ENDPOINT,\ + .ep_in.bmAttributes = USB_EP_TYPE_BULK,\ + .ep_in.bInterval = 0,\ + .ep_out.bLength = sizeof(usb_ep_desc_t),\ + .ep_out.bDescriptorType = USB_DT_ENDPOINT,\ + .ep_out.bmAttributes = USB_EP_TYPE_BULK,\ + .ep_out.bInterval = 0, + +#define UDI_CDC_DATA_DESC_FS(port) { \ + UDI_CDC_DATA_DESC_COMMON \ + .ep_in.wMaxPacketSize = LE16(UDI_CDC_DATA_EPS_FS_SIZE),\ + .ep_out.wMaxPacketSize = LE16(UDI_CDC_DATA_EPS_FS_SIZE),\ + .ep_in.bEndpointAddress = UDI_CDC_DATA_EP_IN_##port,\ + .ep_out.bEndpointAddress = UDI_CDC_DATA_EP_OUT_##port,\ + .iface.bInterfaceNumber = UDI_CDC_DATA_IFACE_NUMBER_##port,\ + .iface.iInterface = UDI_CDC_DATA_STRING_ID_##port,\ + } + +#define UDI_CDC_DATA_DESC_HS(port) { \ + UDI_CDC_DATA_DESC_COMMON \ + .ep_in.wMaxPacketSize = LE16(UDI_CDC_DATA_EPS_HS_SIZE),\ + .ep_out.wMaxPacketSize = LE16(UDI_CDC_DATA_EPS_HS_SIZE),\ + .ep_in.bEndpointAddress = UDI_CDC_DATA_EP_IN_##port,\ + .ep_out.bEndpointAddress = UDI_CDC_DATA_EP_OUT_##port,\ + .iface.bInterfaceNumber = UDI_CDC_DATA_IFACE_NUMBER_##port,\ + .iface.iInterface = UDI_CDC_DATA_STRING_ID_##port,\ + } + +//@} + +/** + * \ingroup udi_group + * \defgroup udi_cdc_group USB Device Interface (UDI) for Communication Class Device (CDC) + * + * Common APIs used by high level application to use this USB class. + * + * These routines are used to transfer and control data + * to/from USB CDC endpoint. + * + * See \ref udi_cdc_quickstart. + * @{ + */ + +/** + * \name Interface for application with single CDC interface support + */ +//@{ + +/** + * \brief Notify a state change of DCD signal + * + * \param b_set DCD is enabled if true, else disabled + */ +void udi_cdc_ctrl_signal_dcd(bool b_set); + +/** + * \brief Notify a state change of DSR signal + * + * \param b_set DSR is enabled if true, else disabled + */ +void udi_cdc_ctrl_signal_dsr(bool b_set); + +/** + * \brief Notify a framing error + */ +void udi_cdc_signal_framing_error(void); + +/** + * \brief Notify a parity error + */ +void udi_cdc_signal_parity_error(void); + +/** + * \brief Notify a overrun + */ +void udi_cdc_signal_overrun(void); + +/** + * \brief Gets the number of byte received + * + * \return the number of data available + */ +iram_size_t udi_cdc_get_nb_received_data(void); + +/** + * \brief This function checks if a character has been received on the CDC line + * + * \return \c 1 if a byte is ready to be read. + */ +bool udi_cdc_is_rx_ready(void); + +/** + * \brief Waits and gets a value on CDC line + * + * \return value read on CDC line + */ +int udi_cdc_getc(void); + +/** + * \brief Reads a RAM buffer on CDC line + * + * \param buf Values read + * \param size Number of value read + * + * \return the number of data remaining + */ +iram_size_t udi_cdc_read_buf(void* buf, iram_size_t size); + +/** + * \brief Non polling reads of a up to 'size' data from CDC line + * + * \param port Communication port number to manage + * \param buf Buffer where to store read data + * \param size Maximum number of data to read (size of buffer) + * + * \return the number of data effectively read + */ +iram_size_t udi_cdc_read_no_polling(void* buf, iram_size_t size); + +/** + * \brief Gets the number of free byte in TX buffer + * + * \return the number of free byte in TX buffer + */ +iram_size_t udi_cdc_get_free_tx_buffer(void); + +/** + * \brief This function checks if a new character sent is possible + * The type int is used to support scanf redirection from compiler LIB. + * + * \return \c 1 if a new character can be sent + */ +bool udi_cdc_is_tx_ready(void); + +/** + * \brief Puts a byte on CDC line + * The type int is used to support printf redirection from compiler LIB. + * + * \param value Value to put + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +int udi_cdc_putc(int value); + +/** + * \brief Writes a RAM buffer on CDC line + * + * \param buf Values to write + * \param size Number of value to write + * + * \return the number of data remaining + */ +iram_size_t udi_cdc_write_buf(const void* buf, iram_size_t size); +//@} + +/** + * \name Interface for application with multi CDC interfaces support + */ +//@{ + +/** + * \brief Notify a state change of DCD signal + * + * \param port Communication port number to manage + * \param b_set DCD is enabled if true, else disabled + */ +void udi_cdc_multi_ctrl_signal_dcd(uint8_t port, bool b_set); + +/** + * \brief Notify a state change of DSR signal + * + * \param port Communication port number to manage + * \param b_set DSR is enabled if true, else disabled + */ +void udi_cdc_multi_ctrl_signal_dsr(uint8_t port, bool b_set); + +/** + * \brief Notify a framing error + * + * \param port Communication port number to manage + */ +void udi_cdc_multi_signal_framing_error(uint8_t port); + +/** + * \brief Notify a parity error + * + * \param port Communication port number to manage + */ +void udi_cdc_multi_signal_parity_error(uint8_t port); + +/** + * \brief Notify a overrun + * + * \param port Communication port number to manage + */ +void udi_cdc_multi_signal_overrun(uint8_t port); + +/** + * \brief Gets the number of byte received + * + * \param port Communication port number to manage + * + * \return the number of data available + */ +iram_size_t udi_cdc_multi_get_nb_received_data(uint8_t port); + +/** + * \brief This function checks if a character has been received on the CDC line + * + * \param port Communication port number to manage + * + * \return \c 1 if a byte is ready to be read. + */ +bool udi_cdc_multi_is_rx_ready(uint8_t port); + +/** + * \brief Waits and gets a value on CDC line + * + * \param port Communication port number to manage + * + * \return value read on CDC line + */ +int udi_cdc_multi_getc(uint8_t port); + +/** + * \brief Reads a RAM buffer on CDC line + * + * \param port Communication port number to manage + * \param buf Values read + * \param size Number of values read + * + * \return the number of data remaining + */ +iram_size_t udi_cdc_multi_read_buf(uint8_t port, void* buf, iram_size_t size); + +/** + * \brief Gets the number of free byte in TX buffer + * + * \param port Communication port number to manage + * + * \return the number of free byte in TX buffer + */ +iram_size_t udi_cdc_multi_get_free_tx_buffer(uint8_t port); + +/** + * \brief This function checks if a new character sent is possible + * The type int is used to support scanf redirection from compiler LIB. + * + * \param port Communication port number to manage + * + * \return \c 1 if a new character can be sent + */ +bool udi_cdc_multi_is_tx_ready(uint8_t port); + +/** + * \brief Puts a byte on CDC line + * The type int is used to support printf redirection from compiler LIB. + * + * \param port Communication port number to manage + * \param value Value to put + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +int udi_cdc_multi_putc(uint8_t port, int value); + +/** + * \brief Writes a RAM buffer on CDC line + * + * \param port Communication port number to manage + * \param buf Values to write + * \param size Number of value to write + * + * \return the number of data remaining + */ +iram_size_t udi_cdc_multi_write_buf(uint8_t port, const void* buf, iram_size_t size); +//@} + +//@} + +/** + * \page udi_cdc_quickstart Quick start guide for USB device Communication Class Device module (UDI CDC) + * + * This is the quick start guide for the \ref udi_cdc_group + * "USB device interface CDC module (UDI CDC)" with step-by-step instructions on + * how to configure and use the modules in a selection of use cases. + * + * The use cases contain several code fragments. The code fragments in the + * steps for setup can be copied into a custom initialization function, while + * the steps for usage can be copied into, e.g., the main application function. + * + * \section udi_cdc_basic_use_case Basic use case + * In this basic use case, the "USB CDC (Single Interface Device)" module is used + * with only one communication port. + * The "USB CDC (Composite Device)" module usage is described in \ref udi_cdc_use_cases + * "Advanced use cases". + * + * \section udi_cdc_basic_use_case_setup Setup steps + * \subsection udi_cdc_basic_use_case_setup_prereq Prerequisites + * \copydetails udc_basic_use_case_setup_prereq + * \subsection udi_cdc_basic_use_case_setup_code Example code + * \copydetails udc_basic_use_case_setup_code + * \subsection udi_cdc_basic_use_case_setup_flow Workflow + * \copydetails udc_basic_use_case_setup_flow + * + * \section udi_cdc_basic_use_case_usage Usage steps + * + * \subsection udi_cdc_basic_use_case_usage_code Example code + * Content of conf_usb.h: + * \code + #define UDI_CDC_ENABLE_EXT(port) my_callback_cdc_enable() + extern bool my_callback_cdc_enable(void); + #define UDI_CDC_DISABLE_EXT(port) my_callback_cdc_disable() + extern void my_callback_cdc_disable(void); + #define UDI_CDC_LOW_RATE + + #define UDI_CDC_DEFAULT_RATE 115200 + #define UDI_CDC_DEFAULT_STOPBITS CDC_STOP_BITS_1 + #define UDI_CDC_DEFAULT_PARITY CDC_PAR_NONE + #define UDI_CDC_DEFAULT_DATABITS 8 + + #include "udi_cdc_conf.h" // At the end of conf_usb.h file +\endcode + * + * Add to application C-file: + * \code + static bool my_flag_autorize_cdc_transfert = false; + bool my_callback_cdc_enable(void) + { + my_flag_autorize_cdc_transfert = true; + return true; + } + void my_callback_cdc_disable(void) + { + my_flag_autorize_cdc_transfert = false; + } + + void task(void) + { + if (my_flag_autorize_cdc_transfert) { + udi_cdc_putc('A'); + udi_cdc_getc(); + } + } +\endcode + * + * \subsection udi_cdc_basic_use_case_setup_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following configuration, + * which is the USB device CDC configuration: + * - \code #define USB_DEVICE_SERIAL_NAME "12...EF" // Disk SN for CDC \endcode + * \note The USB serial number is mandatory when a CDC interface is used. + * - \code #define UDI_CDC_ENABLE_EXT(port) my_callback_cdc_enable() + extern bool my_callback_cdc_enable(void); \endcode + * \note After the device enumeration (detecting and identifying USB devices), + * the USB host starts the device configuration. When the USB CDC interface + * from the device is accepted by the host, the USB host enables this interface and the + * UDI_CDC_ENABLE_EXT() callback function is called and return true. + * Thus, when this event is received, the data transfer on CDC interface are authorized. + * - \code #define UDI_CDC_DISABLE_EXT(port) my_callback_cdc_disable() + extern void my_callback_cdc_disable(void); \endcode + * \note When the USB device is unplugged or is reset by the USB host, the USB + * interface is disabled and the UDI_CDC_DISABLE_EXT() callback function + * is called. Thus, the data transfer must be stopped on CDC interface. + * - \code #define UDI_CDC_LOW_RATE \endcode + * \note Define it when the transfer CDC Device to Host is a low rate + * (<512000 bauds) to reduce CDC buffers size. + * - \code #define UDI_CDC_DEFAULT_RATE 115200 + #define UDI_CDC_DEFAULT_STOPBITS CDC_STOP_BITS_1 + #define UDI_CDC_DEFAULT_PARITY CDC_PAR_NONE + #define UDI_CDC_DEFAULT_DATABITS 8 \endcode + * \note Default configuration of communication port at startup. + * -# Send or wait data on CDC line: + * - \code // Waits and gets a value on CDC line + int udi_cdc_getc(void); + // Reads a RAM buffer on CDC line + iram_size_t udi_cdc_read_buf(int *buf, iram_size_t size); + // Puts a byte on CDC line + int udi_cdc_putc(int value); + // Writes a RAM buffer on CDC line + iram_size_t udi_cdc_write_buf(const int *buf, iram_size_t size); \endcode + * + * \section udi_cdc_use_cases Advanced use cases + * For more advanced use of the UDI CDC module, see the following use cases: + * - \subpage udi_cdc_use_case_composite + * - \subpage udc_use_case_1 + * - \subpage udc_use_case_2 + * - \subpage udc_use_case_3 + * - \subpage udc_use_case_4 + * - \subpage udc_use_case_5 + * - \subpage udc_use_case_6 + */ + +/** + * \page udi_cdc_use_case_composite CDC in a composite device + * + * A USB Composite Device is a USB Device which uses more than one USB class. + * In this use case, the "USB CDC (Composite Device)" module is used to + * create a USB composite device. Thus, this USB module can be associated with + * another "Composite Device" module, like "USB HID Mouse (Composite Device)". + * + * Also, you can refer to application note + * + * AVR4902 ASF - USB Composite Device. + * + * \section udi_cdc_use_case_composite_setup Setup steps + * For the setup code of this use case to work, the + * \ref udi_cdc_basic_use_case "basic use case" must be followed. + * + * \section udi_cdc_use_case_composite_usage Usage steps + * + * \subsection udi_cdc_use_case_composite_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_EP_CTRL_SIZE 64 + #define USB_DEVICE_NB_INTERFACE (X+2) + #define USB_DEVICE_MAX_EP (X+3) + + #define UDI_CDC_DATA_EP_IN_0 (1 | USB_EP_DIR_IN) // TX + #define UDI_CDC_DATA_EP_OUT_0 (2 | USB_EP_DIR_OUT) // RX + #define UDI_CDC_COMM_EP_0 (3 | USB_EP_DIR_IN) // Notify endpoint + #define UDI_CDC_COMM_IFACE_NUMBER_0 X+0 + #define UDI_CDC_DATA_IFACE_NUMBER_0 X+1 + + #define UDI_COMPOSITE_DESC_T \ + usb_iad_desc_t udi_cdc_iad; \ + udi_cdc_comm_desc_t udi_cdc_comm; \ + udi_cdc_data_desc_t udi_cdc_data; \ + ... + #define UDI_COMPOSITE_DESC_FS \ + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_FS, \ + ... + #define UDI_COMPOSITE_DESC_HS \ + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_HS, \ + ... + #define UDI_COMPOSITE_API \ + &udi_api_cdc_comm, \ + &udi_api_cdc_data, \ + ... +\endcode + * + * \subsection udi_cdc_use_case_composite_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required for a USB composite device configuration: + * - \code // Endpoint control size, This must be: + // - 8, 16, 32 or 64 for full speed device (8 is recommended to save RAM) + // - 64 for a high speed device + #define USB_DEVICE_EP_CTRL_SIZE 64 + // Total Number of interfaces on this USB device. + // Add 2 for CDC. + #define USB_DEVICE_NB_INTERFACE (X+2) + // Total number of endpoints on this USB device. + // This must include each endpoint for each interface. + // Add 3 for CDC. + #define USB_DEVICE_MAX_EP (X+3) \endcode + * -# Ensure that conf_usb.h contains the description of + * composite device: + * - \code // The endpoint numbers chosen by you for the CDC. + // The endpoint numbers starting from 1. + #define UDI_CDC_DATA_EP_IN_0 (1 | USB_EP_DIR_IN) // TX + #define UDI_CDC_DATA_EP_OUT_0 (2 | USB_EP_DIR_OUT) // RX + #define UDI_CDC_COMM_EP_0 (3 | USB_EP_DIR_IN) // Notify endpoint + // The interface index of an interface starting from 0 + #define UDI_CDC_COMM_IFACE_NUMBER_0 X+0 + #define UDI_CDC_DATA_IFACE_NUMBER_0 X+1 \endcode + * -# Ensure that conf_usb.h contains the following parameters + * required for a USB composite device configuration: + * - \code // USB Interfaces descriptor structure + #define UDI_COMPOSITE_DESC_T \ + ... + usb_iad_desc_t udi_cdc_iad; \ + udi_cdc_comm_desc_t udi_cdc_comm; \ + udi_cdc_data_desc_t udi_cdc_data; \ + ... + // USB Interfaces descriptor value for Full Speed + #define UDI_COMPOSITE_DESC_FS \ + ... + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_FS, \ + ... + // USB Interfaces descriptor value for High Speed + #define UDI_COMPOSITE_DESC_HS \ + ... + .udi_cdc_iad = UDI_CDC_IAD_DESC_0, \ + .udi_cdc_comm = UDI_CDC_COMM_DESC_0, \ + .udi_cdc_data = UDI_CDC_DATA_DESC_0_HS, \ + ... + // USB Interface APIs + #define UDI_COMPOSITE_API \ + ... + &udi_api_cdc_comm, \ + &udi_api_cdc_data, \ + ... \endcode + * - \note The descriptors order given in the four lists above must be the + * same as the order defined by all interface indexes. The interface index + * orders are defined through UDI_X_IFACE_NUMBER defines.\n + * Also, the CDC requires a USB Interface Association Descriptor (IAD) for + * composite device. + */ + +#ifdef __cplusplus +} +#endif +#endif // _UDI_CDC_H_ diff --git a/src/HAL/DUE/usb/udi_cdc_conf.h b/src/HAL/DUE/usb/udi_cdc_conf.h new file mode 100644 index 0000000..e61b8cb --- /dev/null +++ b/src/HAL/DUE/usb/udi_cdc_conf.h @@ -0,0 +1,156 @@ +/** + * \file + * + * \brief Default CDC configuration for a USB Device with a single interface + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDI_CDC_CONF_H_ +#define _UDI_CDC_CONF_H_ + +#include "usb_protocol_cdc.h" +#include "conf_usb.h" + +#ifndef UDI_CDC_PORT_NB +# define UDI_CDC_PORT_NB 1 +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \addtogroup udi_cdc_group_single_desc + * @{ + */ + +//! Control endpoint size (Endpoint 0) +#define USB_DEVICE_EP_CTRL_SIZE 64 + +#if XMEGA +/** + * \name Endpoint configuration on XMEGA + * The XMEGA supports a IN and OUT endpoint with the same number endpoint, + * thus XMEGA can support up to 7 CDC interfaces. + */ +//@{ +#define UDI_CDC_DATA_EP_IN_0 ( 1 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_0 ( 2 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_0 ( 2 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_1 ( 3 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_1 ( 4 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_1 ( 4 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_2 ( 5 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_2 ( 6 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_2 ( 6 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_3 ( 7 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_3 ( 8 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_3 ( 8 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_4 ( 9 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_4 (10 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_4 (10 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_5 (11 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_5 (12 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_5 (12 | USB_EP_DIR_IN) // Notify endpoint +#define UDI_CDC_DATA_EP_IN_6 (13 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_6 (14 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_6 (14 | USB_EP_DIR_IN) // Notify endpoint +//! 2 endpoints numbers used per CDC interface +#define USB_DEVICE_MAX_EP (2*UDI_CDC_PORT_NB) +//@} + +#else + +/** + * \name Default endpoint configuration + * The USBB, UDP, UDPHS and UOTGHS interfaces can support up to 2 CDC interfaces. + */ +//@{ +# if UDI_CDC_PORT_NB > 2 +# error USBB, UDP, UDPHS and UOTGHS interfaces have not enough endpoints. +# endif +#define UDI_CDC_DATA_EP_IN_0 (1 | USB_EP_DIR_IN) // TX +#define UDI_CDC_DATA_EP_OUT_0 (2 | USB_EP_DIR_OUT) // RX +#define UDI_CDC_COMM_EP_0 (3 | USB_EP_DIR_IN) // Notify endpoint +# if SAM3U + /* For 3U max endpoint size of 4 is 64, use 5 and 6 as bulk tx and rx */ +# define UDI_CDC_DATA_EP_IN_1 (6 | USB_EP_DIR_IN) // TX +# define UDI_CDC_DATA_EP_OUT_1 (5 | USB_EP_DIR_OUT) // RX +# define UDI_CDC_COMM_EP_1 (4 | USB_EP_DIR_IN) // Notify +# else +# define UDI_CDC_DATA_EP_IN_1 (4 | USB_EP_DIR_IN) // TX +# define UDI_CDC_DATA_EP_OUT_1 (5 | USB_EP_DIR_OUT) // RX +# define UDI_CDC_COMM_EP_1 (6 | USB_EP_DIR_IN) // Notify +# endif +//! 3 endpoints used per CDC interface +#undef USB_DEVICE_MAX_EP // undefine this definition in header file +#define USB_DEVICE_MAX_EP (3*UDI_CDC_PORT_NB) +//@} + +#endif + +/** + * \name Default Interface numbers + */ +//@{ +#define UDI_CDC_COMM_IFACE_NUMBER_0 0 +#define UDI_CDC_DATA_IFACE_NUMBER_0 1 +#define UDI_CDC_COMM_IFACE_NUMBER_1 2 +#define UDI_CDC_DATA_IFACE_NUMBER_1 3 +#define UDI_CDC_COMM_IFACE_NUMBER_2 4 +#define UDI_CDC_DATA_IFACE_NUMBER_2 5 +#define UDI_CDC_COMM_IFACE_NUMBER_3 6 +#define UDI_CDC_DATA_IFACE_NUMBER_3 7 +#define UDI_CDC_COMM_IFACE_NUMBER_4 8 +#define UDI_CDC_DATA_IFACE_NUMBER_4 9 +#define UDI_CDC_COMM_IFACE_NUMBER_5 10 +#define UDI_CDC_DATA_IFACE_NUMBER_5 11 +#define UDI_CDC_COMM_IFACE_NUMBER_6 12 +#define UDI_CDC_DATA_IFACE_NUMBER_6 13 +//@} + +//@} + +#ifdef __cplusplus +} +#endif +#endif // _UDI_CDC_CONF_H_ diff --git a/src/HAL/DUE/usb/udi_cdc_desc.c b/src/HAL/DUE/usb/udi_cdc_desc.c new file mode 100644 index 0000000..97c334e --- /dev/null +++ b/src/HAL/DUE/usb/udi_cdc_desc.c @@ -0,0 +1,261 @@ +/** + * \file + * + * \brief Default descriptors for a USB Device with a single interface CDC + * + * Copyright (c) 2009-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "conf_usb.h" +#include "udd.h" +#include "udc_desc.h" +#include "udi_cdc.h" + +#if DISABLED(SDSUPPORT) + +/** + * \defgroup udi_cdc_group_single_desc USB device descriptors for a single interface + * + * The following structures provide the USB device descriptors required for + * USB Device with a single interface CDC. + * + * It is ready to use and do not require more definition. + * + * @{ + */ + +//! Two interfaces for a CDC device +#define USB_DEVICE_NB_INTERFACE (2*UDI_CDC_PORT_NB) + +#ifdef USB_DEVICE_LPM_SUPPORT +# define USB_VERSION USB_V2_1 +#else +# define USB_VERSION USB_V2_0 +#endif + +//! USB Device Descriptor +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE usb_dev_desc_t udc_device_desc = { + .bLength = sizeof(usb_dev_desc_t), + .bDescriptorType = USB_DT_DEVICE, + .bcdUSB = LE16(USB_VERSION), +#if UDI_CDC_PORT_NB > 1 + .bDeviceClass = 0, +#else + .bDeviceClass = CDC_CLASS_DEVICE, +#endif + .bDeviceSubClass = 0, + .bDeviceProtocol = 0, + .bMaxPacketSize0 = USB_DEVICE_EP_CTRL_SIZE, + .idVendor = LE16(USB_DEVICE_VENDOR_ID), + .idProduct = LE16(USB_DEVICE_PRODUCT_ID), + .bcdDevice = LE16((USB_DEVICE_MAJOR_VERSION << 8) + | USB_DEVICE_MINOR_VERSION), +#ifdef USB_DEVICE_MANUFACTURE_NAME + .iManufacturer = 1, +#else + .iManufacturer = 0, // No manufacture string +#endif +#ifdef USB_DEVICE_PRODUCT_NAME + .iProduct = 2, +#else + .iProduct = 0, // No product string +#endif +#if (defined USB_DEVICE_SERIAL_NAME || defined USB_DEVICE_GET_SERIAL_NAME_POINTER) + .iSerialNumber = 3, +#else + .iSerialNumber = 0, // No serial string +#endif + .bNumConfigurations = 1 +}; + + +#ifdef USB_DEVICE_HS_SUPPORT +//! USB Device Qualifier Descriptor for HS +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE usb_dev_qual_desc_t udc_device_qual = { + .bLength = sizeof(usb_dev_qual_desc_t), + .bDescriptorType = USB_DT_DEVICE_QUALIFIER, + .bcdUSB = LE16(USB_VERSION), +#if UDI_CDC_PORT_NB > 1 + .bDeviceClass = 0, +#else + .bDeviceClass = CDC_CLASS_DEVICE, +#endif + .bDeviceSubClass = 0, + .bDeviceProtocol = 0, + .bMaxPacketSize0 = USB_DEVICE_EP_CTRL_SIZE, + .bNumConfigurations = 1 +}; +#endif + +#ifdef USB_DEVICE_LPM_SUPPORT +//! USB Device Qualifier Descriptor +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE usb_dev_lpm_desc_t udc_device_lpm = { + .bos.bLength = sizeof(usb_dev_bos_desc_t), + .bos.bDescriptorType = USB_DT_BOS, + .bos.wTotalLength = LE16(sizeof(usb_dev_bos_desc_t) + sizeof(usb_dev_capa_ext_desc_t)), + .bos.bNumDeviceCaps = 1, + .capa_ext.bLength = sizeof(usb_dev_capa_ext_desc_t), + .capa_ext.bDescriptorType = USB_DT_DEVICE_CAPABILITY, + .capa_ext.bDevCapabilityType = USB_DC_USB20_EXTENSION, + .capa_ext.bmAttributes = USB_DC_EXT_LPM, +}; +#endif + +//! Structure for USB Device Configuration Descriptor +COMPILER_PACK_SET(1) +typedef struct { + usb_conf_desc_t conf; +#if UDI_CDC_PORT_NB == 1 + udi_cdc_comm_desc_t udi_cdc_comm_0; + udi_cdc_data_desc_t udi_cdc_data_0; +#else +# define UDI_CDC_DESC_STRUCTURE(index, unused) \ + usb_iad_desc_t udi_cdc_iad_##index; \ + udi_cdc_comm_desc_t udi_cdc_comm_##index; \ + udi_cdc_data_desc_t udi_cdc_data_##index; + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_DESC_STRUCTURE, ~) +# undef UDI_CDC_DESC_STRUCTURE +#endif +} udc_desc_t; +COMPILER_PACK_RESET() + +//! USB Device Configuration Descriptor filled for full and high speed +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE udc_desc_t udc_desc_fs = { + .conf.bLength = sizeof(usb_conf_desc_t), + .conf.bDescriptorType = USB_DT_CONFIGURATION, + .conf.wTotalLength = LE16(sizeof(udc_desc_t)), + .conf.bNumInterfaces = USB_DEVICE_NB_INTERFACE, + .conf.bConfigurationValue = 1, + .conf.iConfiguration = 0, + .conf.bmAttributes = USB_CONFIG_ATTR_MUST_SET | USB_DEVICE_ATTR, + .conf.bMaxPower = USB_CONFIG_MAX_POWER(USB_DEVICE_POWER), +#if UDI_CDC_PORT_NB == 1 + .udi_cdc_comm_0 = UDI_CDC_COMM_DESC_0, + .udi_cdc_data_0 = UDI_CDC_DATA_DESC_0_FS, +#else +# define UDI_CDC_DESC_FS(index, unused) \ + .udi_cdc_iad_##index = UDI_CDC_IAD_DESC_##index,\ + .udi_cdc_comm_##index = UDI_CDC_COMM_DESC_##index,\ + .udi_cdc_data_##index = UDI_CDC_DATA_DESC_##index##_FS, + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_DESC_FS, ~) +# undef UDI_CDC_DESC_FS +#endif +}; + +#ifdef USB_DEVICE_HS_SUPPORT +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE udc_desc_t udc_desc_hs = { + .conf.bLength = sizeof(usb_conf_desc_t), + .conf.bDescriptorType = USB_DT_CONFIGURATION, + .conf.wTotalLength = LE16(sizeof(udc_desc_t)), + .conf.bNumInterfaces = USB_DEVICE_NB_INTERFACE, + .conf.bConfigurationValue = 1, + .conf.iConfiguration = 0, + .conf.bmAttributes = USB_CONFIG_ATTR_MUST_SET | USB_DEVICE_ATTR, + .conf.bMaxPower = USB_CONFIG_MAX_POWER(USB_DEVICE_POWER), +#if UDI_CDC_PORT_NB == 1 + .udi_cdc_comm_0 = UDI_CDC_COMM_DESC_0, + .udi_cdc_data_0 = UDI_CDC_DATA_DESC_0_HS, +#else +# define UDI_CDC_DESC_HS(index, unused) \ + .udi_cdc_iad_##index = UDI_CDC_IAD_DESC_##index, \ + .udi_cdc_comm_##index = UDI_CDC_COMM_DESC_##index, \ + .udi_cdc_data_##index = UDI_CDC_DATA_DESC_##index##_HS, + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_DESC_HS, ~) +# undef UDI_CDC_DESC_HS +#endif +}; +#endif + +/** + * \name UDC structures which content all USB Device definitions + */ +//@{ + +//! Associate an UDI for each USB interface +UDC_DESC_STORAGE udi_api_t *udi_apis[USB_DEVICE_NB_INTERFACE] = { +# define UDI_CDC_API(index, unused) \ + &udi_api_cdc_comm, \ + &udi_api_cdc_data, + MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_API, ~) +# undef UDI_CDC_API +}; + +//! Add UDI with USB Descriptors FS & HS +UDC_DESC_STORAGE udc_config_speed_t udc_config_fs[1] = { { + .desc = (usb_conf_desc_t UDC_DESC_STORAGE*)&udc_desc_fs, + .udi_apis = udi_apis, +}}; +#ifdef USB_DEVICE_HS_SUPPORT +UDC_DESC_STORAGE udc_config_speed_t udc_config_hs[1] = { { + .desc = (usb_conf_desc_t UDC_DESC_STORAGE*)&udc_desc_hs, + .udi_apis = udi_apis, +}}; +#endif + +//! Add all information about USB Device in global structure for UDC +UDC_DESC_STORAGE udc_config_t udc_config = { + .confdev_lsfs = &udc_device_desc, + .conf_lsfs = udc_config_fs, +#ifdef USB_DEVICE_HS_SUPPORT + .confdev_hs = &udc_device_desc, + .qualifier = &udc_device_qual, + .conf_hs = udc_config_hs, +#endif +#ifdef USB_DEVICE_LPM_SUPPORT + .conf_bos = &udc_device_lpm.bos, +#else + .conf_bos = NULL, +#endif +}; + +//@} +//@} + +#endif // SDSUPPORT + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/udi_composite_desc.c b/src/HAL/DUE/usb/udi_composite_desc.c new file mode 100644 index 0000000..da74fbe --- /dev/null +++ b/src/HAL/DUE/usb/udi_composite_desc.c @@ -0,0 +1,192 @@ +/** + * \file + * + * \brief Descriptors for an USB Composite Device + * + * Copyright (c) 2009-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "conf_usb.h" +#include "udd.h" +#include "udc_desc.h" + +#if ENABLED(SDSUPPORT) + +/** + * \defgroup udi_group_desc Descriptors for a USB Device + * composite + * + * @{ + */ + +/**INDENT-OFF**/ + +//! USB Device Descriptor +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE usb_dev_desc_t udc_device_desc = { + .bLength = sizeof(usb_dev_desc_t), + .bDescriptorType = USB_DT_DEVICE, + .bcdUSB = LE16(USB_V2_0), + .bDeviceClass = CDC_CLASS_MULTI, + .bDeviceSubClass = CDC_SUBCLASS_ACM, + .bDeviceProtocol = CDC_PROTOCOL_V25TER, + .bMaxPacketSize0 = USB_DEVICE_EP_CTRL_SIZE, + .idVendor = LE16(USB_DEVICE_VENDOR_ID), + .idProduct = LE16(USB_DEVICE_PRODUCT_ID), + .bcdDevice = LE16((USB_DEVICE_MAJOR_VERSION << 8) + | USB_DEVICE_MINOR_VERSION), +#ifdef USB_DEVICE_MANUFACTURE_NAME + .iManufacturer = 1, +#else + .iManufacturer = 0, // No manufacture string +#endif +#ifdef USB_DEVICE_PRODUCT_NAME + .iProduct = 2, +#else + .iProduct = 0, // No product string +#endif +#if (defined USB_DEVICE_SERIAL_NAME || defined USB_DEVICE_GET_SERIAL_NAME_POINTER) + .iSerialNumber = 3, +#else + .iSerialNumber = 0, // No serial string +#endif + .bNumConfigurations = 1 +}; + + +#ifdef USB_DEVICE_HS_SUPPORT +//! USB Device Qualifier Descriptor for HS +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE usb_dev_qual_desc_t udc_device_qual = { + .bLength = sizeof(usb_dev_qual_desc_t), + .bDescriptorType = USB_DT_DEVICE_QUALIFIER, + .bcdUSB = LE16(USB_V2_0), + .bDeviceClass = CDC_CLASS_MULTI, + .bDeviceSubClass = CDC_SUBCLASS_ACM, + .bDeviceProtocol = CDC_PROTOCOL_V25TER, + .bMaxPacketSize0 = USB_DEVICE_EP_CTRL_SIZE, + .bNumConfigurations = 1 +}; +#endif + +//! Structure for USB Device Configuration Descriptor +COMPILER_PACK_SET(1) +typedef struct { + usb_conf_desc_t conf; + UDI_COMPOSITE_DESC_T; +} udc_desc_t; +COMPILER_PACK_RESET() + +//! USB Device Configuration Descriptor filled for FS +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE udc_desc_t udc_desc_fs = { + .conf.bLength = sizeof(usb_conf_desc_t), + .conf.bDescriptorType = USB_DT_CONFIGURATION, + .conf.wTotalLength = LE16(sizeof(udc_desc_t)), + .conf.bNumInterfaces = USB_DEVICE_NB_INTERFACE, + .conf.bConfigurationValue = 1, + .conf.iConfiguration = 0, + .conf.bmAttributes = USB_CONFIG_ATTR_MUST_SET | USB_DEVICE_ATTR, + .conf.bMaxPower = USB_CONFIG_MAX_POWER(USB_DEVICE_POWER), + UDI_COMPOSITE_DESC_FS +}; + +#ifdef USB_DEVICE_HS_SUPPORT +//! USB Device Configuration Descriptor filled for HS +COMPILER_WORD_ALIGNED +UDC_DESC_STORAGE udc_desc_t udc_desc_hs = { + .conf.bLength = sizeof(usb_conf_desc_t), + .conf.bDescriptorType = USB_DT_CONFIGURATION, + .conf.wTotalLength = LE16(sizeof(udc_desc_t)), + .conf.bNumInterfaces = USB_DEVICE_NB_INTERFACE, + .conf.bConfigurationValue = 1, + .conf.iConfiguration = 0, + .conf.bmAttributes = USB_CONFIG_ATTR_MUST_SET | USB_DEVICE_ATTR, + .conf.bMaxPower = USB_CONFIG_MAX_POWER(USB_DEVICE_POWER), + UDI_COMPOSITE_DESC_HS +}; +#endif + + +/** + * \name UDC structures which contains all USB Device definitions + */ +//@{ + +//! Associate an UDI for each USB interface +UDC_DESC_STORAGE udi_api_t *udi_apis[USB_DEVICE_NB_INTERFACE] = { + UDI_COMPOSITE_API +}; + +//! Add UDI with USB Descriptors FS +UDC_DESC_STORAGE udc_config_speed_t udc_config_lsfs[1] = {{ + .desc = (usb_conf_desc_t UDC_DESC_STORAGE*)&udc_desc_fs, + .udi_apis = udi_apis, +}}; + +#ifdef USB_DEVICE_HS_SUPPORT +//! Add UDI with USB Descriptors HS +UDC_DESC_STORAGE udc_config_speed_t udc_config_hs[1] = {{ + .desc = (usb_conf_desc_t UDC_DESC_STORAGE*)&udc_desc_hs, + .udi_apis = udi_apis, +}}; +#endif + +//! Add all information about USB Device in global structure for UDC +UDC_DESC_STORAGE udc_config_t udc_config = { + .confdev_lsfs = &udc_device_desc, + .conf_lsfs = udc_config_lsfs, +#ifdef USB_DEVICE_HS_SUPPORT + .confdev_hs = &udc_device_desc, + .qualifier = &udc_device_qual, + .conf_hs = udc_config_hs, +#endif +}; + +//@} +/**INDENT-ON**/ +//@} + +#endif // ARDUINO_ARCH_SAM + +#endif // SDSUPPORT diff --git a/src/HAL/DUE/usb/udi_msc.c b/src/HAL/DUE/usb/udi_msc.c new file mode 100644 index 0000000..dd34048 --- /dev/null +++ b/src/HAL/DUE/usb/udi_msc.c @@ -0,0 +1,1132 @@ +/** + * \file + * + * \brief USB Device Mass Storage Class (MSC) interface. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "usb_protocol_msc.h" +#include "spc_protocol.h" +#include "sbc_protocol.h" +#include "udd.h" +#include "udc.h" +#include "udi_msc.h" +#include "ctrl_access.h" +#include + +#if ENABLED(SDSUPPORT) + +#ifndef UDI_MSC_NOTIFY_TRANS_EXT +# define UDI_MSC_NOTIFY_TRANS_EXT() +#endif + +/** + * \ingroup udi_msc_group + * \defgroup udi_msc_group_udc Interface with USB Device Core (UDC) + * + * Structures and functions required by UDC. + * + * @{ + */ +bool udi_msc_enable(void); +void udi_msc_disable(void); +bool udi_msc_setup(void); +uint8_t udi_msc_getsetting(void); + +//! Global structure which contains standard UDI API for UDC +UDC_DESC_STORAGE udi_api_t udi_api_msc = { + .enable = udi_msc_enable, + .disable = udi_msc_disable, + .setup = udi_msc_setup, + .getsetting = udi_msc_getsetting, + .sof_notify = NULL, +}; +//@} + + +/** + * \ingroup udi_msc_group + * \defgroup udi_msc_group_internal Implementation of UDI MSC + * + * Class internal implementation + * @{ + */ + +//! Static block size for all memories +#define UDI_MSC_BLOCK_SIZE 512L + +/** + * \name Variables to manage SCSI requests + */ +//@{ + +//! Structure to receive a CBW packet +UDC_BSS(4) static struct usb_msc_cbw udi_msc_cbw; +//! Structure to send a CSW packet +UDC_DATA(4) static struct usb_msc_csw udi_msc_csw = + {.dCSWSignature = CPU_TO_BE32(USB_CSW_SIGNATURE) }; +//! Number of lun +UDC_DATA(4) static uint8_t udi_msc_nb_lun = 0; +//! Structure with current SCSI sense data +UDC_BSS(4) static struct scsi_request_sense_data udi_msc_sense; + +/** + * \name Variables to manage the background read/write SCSI commands + */ +//@{ +//! True if an invalid CBW command has been detected +static bool udi_msc_b_cbw_invalid = false; +//! True if a transfer command must be processed +static bool udi_msc_b_trans_req = false; +//! True if it is a read command, else write command +static bool udi_msc_b_read; +//! Memory address to execute the command +static uint32_t udi_msc_addr; +//! Number of block to transfer +static uint16_t udi_msc_nb_block; +//! Signal end of transfer, if true +volatile bool udi_msc_b_ack_trans = true; +//! Status of transfer, aborted if true +volatile bool udi_msc_b_abort_trans; +//! Signal (re)init of transfer, if true (by reset/reconnect) +volatile bool udi_msc_b_reset_trans = true; +//@} + +//@} + + +/** + * \name Internal routines + */ +//@{ + +/** + * \name Routines to process CBW packet + */ +//@{ + +/** + * \brief Stall CBW request + */ +static void udi_msc_cbw_invalid(void); + +/** + * \brief Stall CSW request + */ +static void udi_msc_csw_invalid(void); + +/** + * \brief Links a callback and buffer on endpoint OUT reception + * + * Called by: + * - enable interface + * - at the end of previous command after sending the CSW + */ +static void udi_msc_cbw_wait(void); + +/** + * \brief Callback called after CBW reception + * Called by UDD when a transfer is finished or aborted + * + * \param status UDD_EP_TRANSFER_OK, if transfer is finished + * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted + * \param nb_received number of data transferred + */ +static void udi_msc_cbw_received(udd_ep_status_t status, + iram_size_t nb_received, udd_ep_id_t ep); + +/** + * \brief Function to check the CBW length and direction + * Call it after SCSI command decode to check integrity of command + * + * \param alloc_len number of bytes that device want transfer + * \param dir_flag Direction of transfer (USB_CBW_DIRECTION_IN/OUT) + * + * \retval true if the command can be processed + */ +static bool udi_msc_cbw_validate(uint32_t alloc_len, uint8_t dir_flag); +//@} + + +/** + * \name Routines to process small data packet + */ +//@{ + +/** + * \brief Sends data on MSC IN endpoint + * Called by SCSI command which must send a data to host followed by a CSW + * + * \param buffer Internal RAM buffer to send + * \param buf_size Size of buffer to send + */ +static void udi_msc_data_send(uint8_t * buffer, uint8_t buf_size); + +/** + * \brief Callback called after data sent + * It start CSW packet process + * + * \param status UDD_EP_TRANSFER_OK, if transfer finish + * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted + * \param nb_sent number of data transferred + */ +static void udi_msc_data_sent(udd_ep_status_t status, iram_size_t nb_sent, + udd_ep_id_t ep); +//@} + + +/** + * \name Routines to process CSW packet + */ +//@{ + +/** + * \brief Build CSW packet and send it + * + * Called at the end of SCSI command + */ +static void udi_msc_csw_process(void); + +/** + * \brief Sends CSW + * + * Called by #udi_msc_csw_process() + * or UDD callback when endpoint halt is cleared + */ +void udi_msc_csw_send(void); + +/** + * \brief Callback called after CSW sent + * It restart CBW reception. + * + * \param status UDD_EP_TRANSFER_OK, if transfer is finished + * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted + * \param nb_sent number of data transferred + */ +static void udi_msc_csw_sent(udd_ep_status_t status, iram_size_t nb_sent, + udd_ep_id_t ep); +//@} + + +/** + * \name Routines manage sense data + */ +//@{ + +/** + * \brief Reinitialize sense data. + */ +static void udi_msc_clear_sense(void); + +/** + * \brief Update sense data with new value to signal a fail + * + * \param sense_key Sense key + * \param add_sense Additional Sense Code + * \param lba LBA corresponding at error + */ +static void udi_msc_sense_fail(uint8_t sense_key, uint16_t add_sense, + uint32_t lba); + +/** + * \brief Update sense data with new value to signal success + */ +static void udi_msc_sense_pass(void); + +/** + * \brief Update sense data to signal that memory is not present + */ +static void udi_msc_sense_fail_not_present(void); + +/** + * \brief Update sense data to signal that memory is busy + */ +static void udi_msc_sense_fail_busy_or_change(void); + +/** + * \brief Update sense data to signal a hardware error on memory + */ +static void udi_msc_sense_fail_hardware(void); + +/** + * \brief Update sense data to signal that memory is protected + */ +static void udi_msc_sense_fail_protected(void); + +/** + * \brief Update sense data to signal that CDB fields are not valid + */ +static void udi_msc_sense_fail_cdb_invalid(void); + +/** + * \brief Update sense data to signal that command is not supported + */ +static void udi_msc_sense_command_invalid(void); +//@} + + +/** + * \name Routines manage SCSI Commands + */ +//@{ + +/** + * \brief Process SPC Request Sense command + * Returns error information about last command + */ +static void udi_msc_spc_requestsense(void); + +/** + * \brief Process SPC Inquiry command + * Returns information (name,version) about disk + */ +static void udi_msc_spc_inquiry(void); + +/** + * \brief Checks state of disk + * + * \retval true if disk is ready, otherwise false and updates sense data + */ +static bool udi_msc_spc_testunitready_global(void); + +/** + * \brief Process test unit ready command + * Returns state of logical unit + */ +static void udi_msc_spc_testunitready(void); + +/** + * \brief Process prevent allow medium removal command + */ +static void udi_msc_spc_prevent_allow_medium_removal(void); + +/** + * \brief Process mode sense command + * + * \param b_sense10 Sense10 SCSI command, if true + * \param b_sense10 Sense6 SCSI command, if false + */ +static void udi_msc_spc_mode_sense(bool b_sense10); + +/** + * \brief Process start stop command + */ +static void udi_msc_sbc_start_stop(void); + +/** + * \brief Process read capacity command + */ +static void udi_msc_sbc_read_capacity(void); + +/** + * \brief Process read10 or write10 command + * + * \param b_read Read transfer, if true, + * \param b_read Write transfer, if false + */ +static void udi_msc_sbc_trans(bool b_read); +//@} + +//@} + + +bool udi_msc_enable(void) +{ + uint8_t lun; + udi_msc_b_trans_req = false; + udi_msc_b_cbw_invalid = false; + udi_msc_b_ack_trans = true; + udi_msc_b_reset_trans = true; + udi_msc_nb_lun = get_nb_lun(); + if (0 == udi_msc_nb_lun) + return false; // No lun available, then not authorize to enable interface + udi_msc_nb_lun--; + // Call application callback + // to initialize memories or signal that interface is enabled + if (!UDI_MSC_ENABLE_EXT()) + return false; + // Load the medium on each LUN + for (lun = 0; lun <= udi_msc_nb_lun; lun ++) { + mem_unload(lun, false); + } + // Start MSC process by CBW reception + udi_msc_cbw_wait(); + return true; +} + + +void udi_msc_disable(void) +{ + udi_msc_b_trans_req = false; + udi_msc_b_ack_trans = true; + udi_msc_b_reset_trans = true; + UDI_MSC_DISABLE_EXT(); +} + + +bool udi_msc_setup(void) +{ + if (Udd_setup_is_in()) { + // Requests Interface GET + if (Udd_setup_type() == USB_REQ_TYPE_CLASS) { + // Requests Class Interface Get + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_MSC_GET_MAX_LUN: + // Give the number of memories available + if (1 != udd_g_ctrlreq.req.wLength) + return false; // Error for USB host + if (0 != udd_g_ctrlreq.req.wValue) + return false; + udd_g_ctrlreq.payload = &udi_msc_nb_lun; + udd_g_ctrlreq.payload_size = 1; + return true; + } + } + } + if (Udd_setup_is_out()) { + // Requests Interface SET + if (Udd_setup_type() == USB_REQ_TYPE_CLASS) { + // Requests Class Interface Set + switch (udd_g_ctrlreq.req.bRequest) { + case USB_REQ_MSC_BULK_RESET: + // Reset MSC interface + if (0 != udd_g_ctrlreq.req.wLength) + return false; + if (0 != udd_g_ctrlreq.req.wValue) + return false; + udi_msc_b_cbw_invalid = false; + udi_msc_b_trans_req = false; + // Abort all tasks (transfer or clear stall wait) on endpoints + udd_ep_abort(UDI_MSC_EP_OUT); + udd_ep_abort(UDI_MSC_EP_IN); + // Restart by CBW wait + udi_msc_cbw_wait(); + return true; + } + } + } + return false; // Not supported request +} + +uint8_t udi_msc_getsetting(void) +{ + return 0; // MSC don't have multiple alternate setting +} + + +// ------------------------ +//------- Routines to process CBW packet + +static void udi_msc_cbw_invalid(void) +{ + if (!udi_msc_b_cbw_invalid) + return; // Don't re-stall endpoint if error reset by setup + udd_ep_set_halt(UDI_MSC_EP_OUT); + // If stall cleared then re-stall it. Only Setup MSC Reset can clear it + udd_ep_wait_stall_clear(UDI_MSC_EP_OUT, udi_msc_cbw_invalid); +} + +static void udi_msc_csw_invalid(void) +{ + if (!udi_msc_b_cbw_invalid) + return; // Don't re-stall endpoint if error reset by setup + udd_ep_set_halt(UDI_MSC_EP_IN); + // If stall cleared then re-stall it. Only Setup MSC Reset can clear it + udd_ep_wait_stall_clear(UDI_MSC_EP_IN, udi_msc_csw_invalid); +} + +static void udi_msc_cbw_wait(void) +{ + // Register buffer and callback on OUT endpoint + if (!udd_ep_run(UDI_MSC_EP_OUT, true, + (uint8_t *) & udi_msc_cbw, + sizeof(udi_msc_cbw), + udi_msc_cbw_received)) { + // OUT endpoint not available (halted), then wait a clear of halt. + udd_ep_wait_stall_clear(UDI_MSC_EP_OUT, udi_msc_cbw_wait); + } +} + + +static void udi_msc_cbw_received(udd_ep_status_t status, + iram_size_t nb_received, udd_ep_id_t ep) +{ + UNUSED(ep); + // Check status of transfer + if (UDD_EP_TRANSFER_OK != status) { + // Transfer aborted + // Now wait MSC setup reset to relaunch CBW reception + return; + } + // Check CBW integrity: + // transfer status/CBW length/CBW signature + if ((sizeof(udi_msc_cbw) != nb_received) + || (udi_msc_cbw.dCBWSignature != + CPU_TO_BE32(USB_CBW_SIGNATURE))) { + // (5.2.1) Devices receiving a CBW with an invalid signature should stall + // further traffic on the Bulk In pipe, and either stall further traffic + // or accept and discard further traffic on the Bulk Out pipe, until + // reset recovery. + udi_msc_b_cbw_invalid = true; + udi_msc_cbw_invalid(); + udi_msc_csw_invalid(); + return; + } + // Check LUN asked + udi_msc_cbw.bCBWLUN &= USB_CBW_LUN_MASK; + if (udi_msc_cbw.bCBWLUN > udi_msc_nb_lun) { + // Bad LUN, then stop command process + udi_msc_sense_fail_cdb_invalid(); + udi_msc_csw_process(); + return; + } + // Prepare CSW residue field with the size requested + udi_msc_csw.dCSWDataResidue = + le32_to_cpu(udi_msc_cbw.dCBWDataTransferLength); + + // Decode opcode + switch (udi_msc_cbw.CDB[0]) { + case SPC_REQUEST_SENSE: + udi_msc_spc_requestsense(); + break; + + case SPC_INQUIRY: + udi_msc_spc_inquiry(); + break; + + case SPC_MODE_SENSE6: + udi_msc_spc_mode_sense(false); + break; + case SPC_MODE_SENSE10: + udi_msc_spc_mode_sense(true); + break; + + case SPC_TEST_UNIT_READY: + udi_msc_spc_testunitready(); + break; + + case SBC_READ_CAPACITY10: + udi_msc_sbc_read_capacity(); + break; + + case SBC_START_STOP_UNIT: + udi_msc_sbc_start_stop(); + break; + + // Accepts request to support plug/plug in case of card reader + case SPC_PREVENT_ALLOW_MEDIUM_REMOVAL: + udi_msc_spc_prevent_allow_medium_removal(); + break; + + // Accepts request to support full format from Windows + case SBC_VERIFY10: + udi_msc_sense_pass(); + udi_msc_csw_process(); + break; + + case SBC_READ10: + udi_msc_sbc_trans(true); + break; + + case SBC_WRITE10: + udi_msc_sbc_trans(false); + break; + + default: + udi_msc_sense_command_invalid(); + udi_msc_csw_process(); + break; + } +} + + +static bool udi_msc_cbw_validate(uint32_t alloc_len, uint8_t dir_flag) +{ + /* + * The following cases should result in a phase error: + * - Case 2: Hn < Di + * - Case 3: Hn < Do + * - Case 7: Hi < Di + * - Case 8: Hi <> Do + * - Case 10: Ho <> Di + * - Case 13: Ho < Do + */ + if (((udi_msc_cbw.bmCBWFlags ^ dir_flag) & USB_CBW_DIRECTION_IN) + || (udi_msc_csw.dCSWDataResidue < alloc_len)) { + udi_msc_sense_fail_cdb_invalid(); + udi_msc_csw_process(); + return false; + } + + /* + * The following cases should result in a stall and nonzero + * residue: + * - Case 4: Hi > Dn + * - Case 5: Hi > Di + * - Case 9: Ho > Dn + * - Case 11: Ho > Do + */ + return true; +} + + +// ------------------------ +//------- Routines to process small data packet + +static void udi_msc_data_send(uint8_t * buffer, uint8_t buf_size) +{ + // Sends data on IN endpoint + if (!udd_ep_run(UDI_MSC_EP_IN, true, + buffer, buf_size, udi_msc_data_sent)) { + // If endpoint not available, then exit process command + udi_msc_sense_fail_hardware(); + udi_msc_csw_process(); + } +} + + +static void udi_msc_data_sent(udd_ep_status_t status, iram_size_t nb_sent, + udd_ep_id_t ep) +{ + UNUSED(ep); + if (UDD_EP_TRANSFER_OK != status) { + // Error protocol + // Now wait MSC setup reset to relaunch CBW reception + return; + } + // Update sense data + udi_msc_sense_pass(); + // Update CSW + udi_msc_csw.dCSWDataResidue -= nb_sent; + udi_msc_csw_process(); +} + + +// ------------------------ +//------- Routines to process CSW packet + +static void udi_msc_csw_process(void) +{ + if (0 != udi_msc_csw.dCSWDataResidue) { + // Residue not NULL + // then STALL next request from USB host on corresponding endpoint + if (udi_msc_cbw.bmCBWFlags & USB_CBW_DIRECTION_IN) + udd_ep_set_halt(UDI_MSC_EP_IN); + else + udd_ep_set_halt(UDI_MSC_EP_OUT); + } + // Prepare and send CSW + udi_msc_csw.dCSWTag = udi_msc_cbw.dCBWTag; + udi_msc_csw.dCSWDataResidue = cpu_to_le32(udi_msc_csw.dCSWDataResidue); + udi_msc_csw_send(); +} + + +void udi_msc_csw_send(void) +{ + // Sends CSW on IN endpoint + if (!udd_ep_run(UDI_MSC_EP_IN, false, + (uint8_t *) & udi_msc_csw, + sizeof(udi_msc_csw), + udi_msc_csw_sent)) { + // Endpoint not available + // then restart CSW sent when endpoint IN STALL will be cleared + udd_ep_wait_stall_clear(UDI_MSC_EP_IN, udi_msc_csw_send); + } +} + + +static void udi_msc_csw_sent(udd_ep_status_t status, iram_size_t nb_sent, + udd_ep_id_t ep) +{ + UNUSED(ep); + UNUSED(status); + UNUSED(nb_sent); + // CSW is sent or not + // In all case, restart process and wait CBW + udi_msc_cbw_wait(); +} + + +// ------------------------ +//------- Routines manage sense data + +static void udi_msc_clear_sense(void) +{ + memset((uint8_t*)&udi_msc_sense, 0, sizeof(struct scsi_request_sense_data)); + udi_msc_sense.valid_reponse_code = SCSI_SENSE_VALID | SCSI_SENSE_CURRENT; + udi_msc_sense.AddSenseLen = SCSI_SENSE_ADDL_LEN(sizeof(udi_msc_sense)); +} + +static void udi_msc_sense_fail(uint8_t sense_key, uint16_t add_sense, + uint32_t lba) +{ + udi_msc_clear_sense(); + udi_msc_csw.bCSWStatus = USB_CSW_STATUS_FAIL; + udi_msc_sense.sense_flag_key = sense_key; + udi_msc_sense.information[0] = lba >> 24; + udi_msc_sense.information[1] = lba >> 16; + udi_msc_sense.information[2] = lba >> 8; + udi_msc_sense.information[3] = lba; + udi_msc_sense.AddSenseCode = add_sense >> 8; + udi_msc_sense.AddSnsCodeQlfr = add_sense; +} + +static void udi_msc_sense_pass(void) +{ + udi_msc_clear_sense(); + udi_msc_csw.bCSWStatus = USB_CSW_STATUS_PASS; +} + + +static void udi_msc_sense_fail_not_present(void) +{ + udi_msc_sense_fail(SCSI_SK_NOT_READY, SCSI_ASC_MEDIUM_NOT_PRESENT, 0); +} + +static void udi_msc_sense_fail_busy_or_change(void) +{ + udi_msc_sense_fail(SCSI_SK_UNIT_ATTENTION, + SCSI_ASC_NOT_READY_TO_READY_CHANGE, 0); +} + +static void udi_msc_sense_fail_hardware(void) +{ + udi_msc_sense_fail(SCSI_SK_HARDWARE_ERROR, + SCSI_ASC_NO_ADDITIONAL_SENSE_INFO, 0); +} + +static void udi_msc_sense_fail_protected(void) +{ + udi_msc_sense_fail(SCSI_SK_DATA_PROTECT, SCSI_ASC_WRITE_PROTECTED, 0); +} + +static void udi_msc_sense_fail_cdb_invalid(void) +{ + udi_msc_sense_fail(SCSI_SK_ILLEGAL_REQUEST, + SCSI_ASC_INVALID_FIELD_IN_CDB, 0); +} + +static void udi_msc_sense_command_invalid(void) +{ + udi_msc_sense_fail(SCSI_SK_ILLEGAL_REQUEST, + SCSI_ASC_INVALID_COMMAND_OPERATION_CODE, 0); +} + + +// ------------------------ +//------- Routines manage SCSI Commands + +static void udi_msc_spc_requestsense(void) +{ + uint8_t length = udi_msc_cbw.CDB[4]; + + // Can't send more than sense data length + if (length > sizeof(udi_msc_sense)) + length = sizeof(udi_msc_sense); + + if (!udi_msc_cbw_validate(length, USB_CBW_DIRECTION_IN)) + return; + // Send sense data + udi_msc_data_send((uint8_t*)&udi_msc_sense, length); +} + + +static void udi_msc_spc_inquiry(void) +{ + uint8_t length, i; + UDC_DATA(4) + // Constant inquiry data for all LUNs + static struct scsi_inquiry_data udi_msc_inquiry_data = { + .pq_pdt = SCSI_INQ_PQ_CONNECTED | SCSI_INQ_DT_DIR_ACCESS, + .version = SCSI_INQ_VER_SPC, + .flags3 = SCSI_INQ_RSP_SPC2, + .addl_len = SCSI_INQ_ADDL_LEN(sizeof(struct scsi_inquiry_data)), + .vendor_id = {UDI_MSC_GLOBAL_VENDOR_ID}, + .product_rev = {UDI_MSC_GLOBAL_PRODUCT_VERSION}, + }; + + length = udi_msc_cbw.CDB[4]; + + // Can't send more than inquiry data length + if (length > sizeof(udi_msc_inquiry_data)) + length = sizeof(udi_msc_inquiry_data); + + if (!udi_msc_cbw_validate(length, USB_CBW_DIRECTION_IN)) + return; + if ((0 != (udi_msc_cbw.CDB[1] & (SCSI_INQ_REQ_EVPD | SCSI_INQ_REQ_CMDT))) + || (0 != udi_msc_cbw.CDB[2])) { + // CMDT and EPVD bits are not at 0 + // PAGE or OPERATION CODE fields are not empty + // = No standard inquiry asked + udi_msc_sense_fail_cdb_invalid(); // Command is unsupported + udi_msc_csw_process(); + return; + } + + udi_msc_inquiry_data.flags1 = mem_removal(udi_msc_cbw.bCBWLUN) ? + SCSI_INQ_RMB : 0; + + //* Fill product ID field + // Copy name in product id field + memcpy(udi_msc_inquiry_data.product_id, + mem_name(udi_msc_cbw.bCBWLUN)+1, // To remove first '"' + sizeof(udi_msc_inquiry_data.product_id)); + + // Search end of name '/0' or '"' + i = 0; + while (sizeof(udi_msc_inquiry_data.product_id) != i) { + if ((0 == udi_msc_inquiry_data.product_id[i]) + || ('"' == udi_msc_inquiry_data.product_id[i])) { + break; + } + i++; + } + // Padding with space char + while (sizeof(udi_msc_inquiry_data.product_id) != i) { + udi_msc_inquiry_data.product_id[i] = ' '; + i++; + } + + // Send inquiry data + udi_msc_data_send((uint8_t *) & udi_msc_inquiry_data, length); +} + + +static bool udi_msc_spc_testunitready_global(void) +{ + switch (mem_test_unit_ready(udi_msc_cbw.bCBWLUN)) { + case CTRL_GOOD: + return true; // Don't change sense data + case CTRL_BUSY: + udi_msc_sense_fail_busy_or_change(); + break; + case CTRL_NO_PRESENT: + udi_msc_sense_fail_not_present(); + break; + case CTRL_FAIL: + default: + udi_msc_sense_fail_hardware(); + break; + } + return false; +} + + +static void udi_msc_spc_testunitready(void) +{ + if (udi_msc_spc_testunitready_global()) { + // LUN ready, then update sense data with status pass + udi_msc_sense_pass(); + } + // Send status in CSW packet + udi_msc_csw_process(); +} + + +static void udi_msc_spc_mode_sense(bool b_sense10) +{ + // Union of all mode sense structures + union sense_6_10 { + struct { + struct scsi_mode_param_header6 header; + struct spc_control_page_info_execpt sense_data; + } s6; + struct { + struct scsi_mode_param_header10 header; + struct spc_control_page_info_execpt sense_data; + } s10; + }; + + uint8_t data_sense_lgt; + uint8_t mode; + uint8_t request_lgt; + uint8_t wp; + struct spc_control_page_info_execpt *ptr_mode; + UDC_BSS(4) static union sense_6_10 sense; + + // Clear all fields + memset(&sense, 0, sizeof(sense)); + + // Initialize process + if (b_sense10) { + request_lgt = udi_msc_cbw.CDB[8]; + ptr_mode = &sense.s10.sense_data; + data_sense_lgt = sizeof(struct scsi_mode_param_header10); + } else { + request_lgt = udi_msc_cbw.CDB[4]; + ptr_mode = &sense.s6.sense_data; + data_sense_lgt = sizeof(struct scsi_mode_param_header6); + } + + // No Block descriptor + + // Fill page(s) + mode = udi_msc_cbw.CDB[2] & SCSI_MS_MODE_ALL; + if ((SCSI_MS_MODE_INFEXP == mode) + || (SCSI_MS_MODE_ALL == mode)) { + // Informational exceptions control page (from SPC) + ptr_mode->page_code = + SCSI_MS_MODE_INFEXP; + ptr_mode->page_length = + SPC_MP_INFEXP_PAGE_LENGTH; + ptr_mode->mrie = + SPC_MP_INFEXP_MRIE_NO_SENSE; + data_sense_lgt += sizeof(struct spc_control_page_info_execpt); + } + // Can't send more than mode sense data length + if (request_lgt > data_sense_lgt) + request_lgt = data_sense_lgt; + if (!udi_msc_cbw_validate(request_lgt, USB_CBW_DIRECTION_IN)) + return; + + // Fill mode parameter header length + wp = (mem_wr_protect(udi_msc_cbw.bCBWLUN)) ? SCSI_MS_SBC_WP : 0; + + if (b_sense10) { + sense.s10.header.mode_data_length = + cpu_to_be16((data_sense_lgt - 2)); + //sense.s10.header.medium_type = 0; + sense.s10.header.device_specific_parameter = wp; + //sense.s10.header.block_descriptor_length = 0; + } else { + sense.s6.header.mode_data_length = data_sense_lgt - 1; + //sense.s6.header.medium_type = 0; + sense.s6.header.device_specific_parameter = wp; + //sense.s6.header.block_descriptor_length = 0; + } + + // Send mode sense data + udi_msc_data_send((uint8_t *) & sense, request_lgt); +} + + +static void udi_msc_spc_prevent_allow_medium_removal(void) +{ + uint8_t prevent = udi_msc_cbw.CDB[4]; + if (0 == prevent) { + udi_msc_sense_pass(); + } else { + udi_msc_sense_fail_cdb_invalid(); // Command is unsupported + } + udi_msc_csw_process(); +} + + +static void udi_msc_sbc_start_stop(void) +{ + bool start = 0x1 & udi_msc_cbw.CDB[4]; + bool loej = 0x2 & udi_msc_cbw.CDB[4]; + if (loej) { + mem_unload(udi_msc_cbw.bCBWLUN, !start); + } + udi_msc_sense_pass(); + udi_msc_csw_process(); +} + + +static void udi_msc_sbc_read_capacity(void) +{ + UDC_BSS(4) static struct sbc_read_capacity10_data udi_msc_capacity; + + if (!udi_msc_cbw_validate(sizeof(udi_msc_capacity), + USB_CBW_DIRECTION_IN)) + return; + + // Get capacity of LUN + switch (mem_read_capacity(udi_msc_cbw.bCBWLUN, + &udi_msc_capacity.max_lba)) { + case CTRL_GOOD: + break; + case CTRL_BUSY: + udi_msc_sense_fail_busy_or_change(); + udi_msc_csw_process(); + return; + case CTRL_NO_PRESENT: + udi_msc_sense_fail_not_present(); + udi_msc_csw_process(); + return; + default: + udi_msc_sense_fail_hardware(); + udi_msc_csw_process(); + return; + } + + // Format capacity data + udi_msc_capacity.block_len = CPU_TO_BE32(UDI_MSC_BLOCK_SIZE); + udi_msc_capacity.max_lba = cpu_to_be32(udi_msc_capacity.max_lba); + // Send the corresponding sense data + udi_msc_data_send((uint8_t *) & udi_msc_capacity, + sizeof(udi_msc_capacity)); +} + + +static void udi_msc_sbc_trans(bool b_read) +{ + uint32_t trans_size; + + if (!b_read) { + // Write operation then check Write Protect + if (mem_wr_protect(udi_msc_cbw.bCBWLUN)) { + // Write not authorized + udi_msc_sense_fail_protected(); + udi_msc_csw_process(); + return; + } + } + // Read/Write command fields (address and number of block) + MSB0(udi_msc_addr) = udi_msc_cbw.CDB[2]; + MSB1(udi_msc_addr) = udi_msc_cbw.CDB[3]; + MSB2(udi_msc_addr) = udi_msc_cbw.CDB[4]; + MSB3(udi_msc_addr) = udi_msc_cbw.CDB[5]; + MSB(udi_msc_nb_block) = udi_msc_cbw.CDB[7]; + LSB(udi_msc_nb_block) = udi_msc_cbw.CDB[8]; + + // Compute number of byte to transfer and valid it + trans_size = (uint32_t) udi_msc_nb_block *UDI_MSC_BLOCK_SIZE; + if (!udi_msc_cbw_validate(trans_size, + (b_read) ? USB_CBW_DIRECTION_IN : + USB_CBW_DIRECTION_OUT)) + return; + + // Record transfer request to do it in a task and not under interrupt + udi_msc_b_read = b_read; + udi_msc_b_trans_req = true; + UDI_MSC_NOTIFY_TRANS_EXT(); +} + + +bool udi_msc_process_trans(void) +{ + Ctrl_status status; + + if (!udi_msc_b_trans_req) + return false; // No Transfer request to do + udi_msc_b_trans_req = false; + udi_msc_b_reset_trans = false; + + // Start transfer + if (udi_msc_b_read) { + status = memory_2_usb(udi_msc_cbw.bCBWLUN, udi_msc_addr, + udi_msc_nb_block); + } else { + status = usb_2_memory(udi_msc_cbw.bCBWLUN, udi_msc_addr, + udi_msc_nb_block); + } + + // Check if transfer is aborted by reset + if (udi_msc_b_reset_trans) { + udi_msc_b_reset_trans = false; + return true; + } + + // Check status of transfer + switch (status) { + case CTRL_GOOD: + udi_msc_sense_pass(); + break; + case CTRL_BUSY: + udi_msc_sense_fail_busy_or_change(); + break; + case CTRL_NO_PRESENT: + udi_msc_sense_fail_not_present(); + break; + default: + case CTRL_FAIL: + udi_msc_sense_fail_hardware(); + break; + } + // Send status of transfer in CSW packet + udi_msc_csw_process(); + return true; +} + + +static void udi_msc_trans_ack(udd_ep_status_t status, iram_size_t n, + udd_ep_id_t ep) +{ + UNUSED(ep); + UNUSED(n); + // Update variable to signal the end of transfer + udi_msc_b_abort_trans = (UDD_EP_TRANSFER_OK != status) ? true : false; + udi_msc_b_ack_trans = true; +} + + +bool udi_msc_trans_block(bool b_read, uint8_t * block, iram_size_t block_size, + void (*callback) (udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep)) +{ + if (!udi_msc_b_ack_trans) + return false; // No possible, transfer on going + + // Start transfer Internal RAM<->USB line + udi_msc_b_ack_trans = false; + if (!udd_ep_run((b_read) ? UDI_MSC_EP_IN : UDI_MSC_EP_OUT, + false, + block, + block_size, + (NULL == callback) ? udi_msc_trans_ack : + callback)) { + udi_msc_b_ack_trans = true; + return false; + } + if (NULL == callback) { + while (!udi_msc_b_ack_trans); + if (udi_msc_b_abort_trans) { + return false; + } + udi_msc_csw.dCSWDataResidue -= block_size; + return (!udi_msc_b_abort_trans); + } + udi_msc_csw.dCSWDataResidue -= block_size; + return true; +} + +//@} + +#endif // SDSUPPORT + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/udi_msc.h b/src/HAL/DUE/usb/udi_msc.h new file mode 100644 index 0000000..730dbc8 --- /dev/null +++ b/src/HAL/DUE/usb/udi_msc.h @@ -0,0 +1,376 @@ +/** + * \file + * + * \brief USB Device Mass Storage Class (MSC) interface definitions. + * + * Copyright (c) 2009-2016 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _UDI_MSC_H_ +#define _UDI_MSC_H_ + +#include "conf_usb.h" +#include "usb_protocol.h" +#include "usb_protocol_msc.h" +#include "udd.h" +#include "udc_desc.h" +#include "udi.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * \addtogroup udi_msc_group_udc + * @{ + */ +//! Global structure which contains standard UDI interface for UDC +extern UDC_DESC_STORAGE udi_api_t udi_api_msc; +//@} + +/** + * \ingroup udi_msc_group + * \defgroup udi_msc_group USB interface descriptors + * + * The following structures provide predefined USB interface descriptors. + * It must be used to define the final USB descriptors. + */ +//@{ + +//! Interface descriptor structure for MSC +typedef struct { + usb_iface_desc_t iface; + usb_ep_desc_t ep_in; + usb_ep_desc_t ep_out; +} udi_msc_desc_t; + +//! By default no string associated to this interface +#ifndef UDI_MSC_STRING_ID +#define UDI_MSC_STRING_ID 0 +#endif + +//! MSC endpoints size for full speed +#define UDI_MSC_EPS_SIZE_FS 64 +//! MSC endpoints size for high speed +#define UDI_MSC_EPS_SIZE_HS 512 + +//! Content of MSC interface descriptor for all speeds +#define UDI_MSC_DESC \ + .iface.bLength = sizeof(usb_iface_desc_t),\ + .iface.bDescriptorType = USB_DT_INTERFACE,\ + .iface.bInterfaceNumber = UDI_MSC_IFACE_NUMBER,\ + .iface.bAlternateSetting = 0,\ + .iface.bNumEndpoints = 2,\ + .iface.bInterfaceClass = MSC_CLASS,\ + .iface.bInterfaceSubClass = MSC_SUBCLASS_TRANSPARENT,\ + .iface.bInterfaceProtocol = MSC_PROTOCOL_BULK,\ + .iface.iInterface = UDI_MSC_STRING_ID,\ + .ep_in.bLength = sizeof(usb_ep_desc_t),\ + .ep_in.bDescriptorType = USB_DT_ENDPOINT,\ + .ep_in.bEndpointAddress = UDI_MSC_EP_IN,\ + .ep_in.bmAttributes = USB_EP_TYPE_BULK,\ + .ep_in.bInterval = 0,\ + .ep_out.bLength = sizeof(usb_ep_desc_t),\ + .ep_out.bDescriptorType = USB_DT_ENDPOINT,\ + .ep_out.bEndpointAddress = UDI_MSC_EP_OUT,\ + .ep_out.bmAttributes = USB_EP_TYPE_BULK,\ + .ep_out.bInterval = 0, + +//! Content of MSC interface descriptor for full speed only +#define UDI_MSC_DESC_FS {\ + UDI_MSC_DESC \ + .ep_in.wMaxPacketSize = LE16(UDI_MSC_EPS_SIZE_FS),\ + .ep_out.wMaxPacketSize = LE16(UDI_MSC_EPS_SIZE_FS),\ + } + +//! Content of MSC interface descriptor for high speed only +#define UDI_MSC_DESC_HS {\ + UDI_MSC_DESC \ + .ep_in.wMaxPacketSize = LE16(UDI_MSC_EPS_SIZE_HS),\ + .ep_out.wMaxPacketSize = LE16(UDI_MSC_EPS_SIZE_HS),\ + } +//@} + + +/** + * \ingroup udi_group + * \defgroup udi_msc_group USB Device Interface (UDI) for Mass Storage Class (MSC) + * + * Common APIs used by high level application to use this USB class. + * + * These routines are used by memory to transfer its data + * to/from USB MSC endpoints. + * + * See \ref udi_msc_quickstart. + * @{ + */ + +/** + * \brief Process the background read/write commands + * + * Routine called by the main loop + */ +bool udi_msc_process_trans(void); + +/** + * \brief Transfers data to/from USB MSC endpoints + * + * + * \param b_read Memory to USB, if true + * \param block Buffer on Internal RAM to send or fill + * \param block_size Buffer size to send or fill + * \param callback Function to call at the end of transfer. + * If NULL then the routine exit when transfer is finish. + * + * \return \c 1 if function was successfully done, otherwise \c 0. + */ +bool udi_msc_trans_block(bool b_read, uint8_t * block, iram_size_t block_size, + void (*callback) (udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep)); +//@} + +#ifdef __cplusplus +} +#endif + + +/** + * \page udi_msc_quickstart Quick start guide for USB device Mass Storage module (UDI MSC) + * + * This is the quick start guide for the \ref udi_msc_group + * "USB device interface MSC module (UDI MSC)" with step-by-step instructions on + * how to configure and use the modules in a selection of use cases. + * + * The use cases contain several code fragments. The code fragments in the + * steps for setup can be copied into a custom initialization function, while + * the steps for usage can be copied into, e.g., the main application function. + * + * \section udi_msc_basic_use_case Basic use case + * In this basic use case, the "USB MSC (Single Interface Device)" module is used. + * The "USB MSC (Composite Device)" module usage is described in \ref udi_msc_use_cases + * "Advanced use cases". + * + * \section udi_msc_basic_use_case_setup Setup steps + * \subsection udi_msc_basic_use_case_setup_prereq Prerequisites + * \copydetails udc_basic_use_case_setup_prereq + * \subsection udi_msc_basic_use_case_setup_code Example code + * \copydetails udc_basic_use_case_setup_code + * \subsection udi_msc_basic_use_case_setup_flow Workflow + * \copydetails udc_basic_use_case_setup_flow + * + * \section udi_msc_basic_use_case_usage Usage steps + * + * \subsection udi_msc_basic_use_case_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_SERIAL_NAME "12...EF" // Disk SN for MSC + #define UDI_MSC_GLOBAL_VENDOR_ID \ + 'A', 'T', 'M', 'E', 'L', ' ', ' ', ' ' + #define UDI_MSC_GLOBAL_PRODUCT_VERSION \ + '1', '.', '0', '0' + #define UDI_MSC_ENABLE_EXT() my_callback_msc_enable() + extern bool my_callback_msc_enable(void); + #define UDI_MSC_DISABLE_EXT() my_callback_msc_disable() + extern void my_callback_msc_disable(void); + #include "udi_msc_conf.h" // At the end of conf_usb.h file +\endcode + * + * Add to application C-file: + * \code + static bool my_flag_autorize_msc_transfert = false; + bool my_callback_msc_enable(void) + { + my_flag_autorize_msc_transfert = true; + return true; + } + void my_callback_msc_disable(void) + { + my_flag_autorize_msc_transfert = false; + } + + void task(void) + { + udi_msc_process_trans(); + } +\endcode + * + * \subsection udi_msc_basic_use_case_setup_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following configuration, + * which is the USB device MSC configuration: + * - \code #define USB_DEVICE_SERIAL_NAME "12...EF" // Disk SN for MSC \endcode + * \note The USB serial number is mandatory when a MSC interface is used. + * - \code //! Vendor name and Product version of MSC interface + #define UDI_MSC_GLOBAL_VENDOR_ID \ + 'A', 'T', 'M', 'E', 'L', ' ', ' ', ' ' + #define UDI_MSC_GLOBAL_PRODUCT_VERSION \ + '1', '.', '0', '0' \endcode + * \note The USB MSC interface requires a vendor ID (8 ASCII characters) + * and a product version (4 ASCII characters). + * - \code #define UDI_MSC_ENABLE_EXT() my_callback_msc_enable() + extern bool my_callback_msc_enable(void); \endcode + * \note After the device enumeration (detecting and identifying USB devices), + * the USB host starts the device configuration. When the USB MSC interface + * from the device is accepted by the host, the USB host enables this interface and the + * UDI_MSC_ENABLE_EXT() callback function is called and return true. + * Thus, when this event is received, the tasks which call + * udi_msc_process_trans() must be enabled. + * - \code #define UDI_MSC_DISABLE_EXT() my_callback_msc_disable() + extern void my_callback_msc_disable(void); \endcode + * \note When the USB device is unplugged or is reset by the USB host, the USB + * interface is disabled and the UDI_MSC_DISABLE_EXT() callback function + * is called. Thus, it is recommended to disable the task which is called udi_msc_process_trans(). + * -# The MSC is automatically linked with memory control access component + * which provides the memories interfaces. However, the memory data transfers + * must be done outside USB interrupt routine. This is done in the MSC process + * ("udi_msc_process_trans()") called by main loop: + * - \code * void task(void) { + udi_msc_process_trans(); + } \endcode + * -# The MSC speed depends on task periodicity. To get the best speed + * the notification callback "UDI_MSC_NOTIFY_TRANS_EXT" can be used to wakeup + * this task (Example, through a mutex): + * - \code #define UDI_MSC_NOTIFY_TRANS_EXT() msc_notify_trans() + void msc_notify_trans(void) { + wakeup_my_task(); + } \endcode + * + * \section udi_msc_use_cases Advanced use cases + * For more advanced use of the UDI MSC module, see the following use cases: + * - \subpage udi_msc_use_case_composite + * - \subpage udc_use_case_1 + * - \subpage udc_use_case_2 + * - \subpage udc_use_case_3 + * - \subpage udc_use_case_5 + * - \subpage udc_use_case_6 + */ + +/** + * \page udi_msc_use_case_composite MSC in a composite device + * + * A USB Composite Device is a USB Device which uses more than one USB class. + * In this use case, the "USB MSC (Composite Device)" module is used to + * create a USB composite device. Thus, this USB module can be associated with + * another "Composite Device" module, like "USB HID Mouse (Composite Device)". + * + * Also, you can refer to application note + * + * AVR4902 ASF - USB Composite Device. + * + * \section udi_msc_use_case_composite_setup Setup steps + * For the setup code of this use case to work, the + * \ref udi_msc_basic_use_case "basic use case" must be followed. + * + * \section udi_msc_use_case_composite_usage Usage steps + * + * \subsection udi_msc_use_case_composite_usage_code Example code + * Content of conf_usb.h: + * \code + #define USB_DEVICE_EP_CTRL_SIZE 64 + #define USB_DEVICE_NB_INTERFACE (X+1) + #define USB_DEVICE_MAX_EP (X+2) + + #define UDI_MSC_EP_IN (X | USB_EP_DIR_IN) + #define UDI_MSC_EP_OUT (Y | USB_EP_DIR_OUT) + #define UDI_MSC_IFACE_NUMBER X + + #define UDI_COMPOSITE_DESC_T \ + udi_msc_desc_t udi_msc; \ + ... + #define UDI_COMPOSITE_DESC_FS \ + .udi_msc = UDI_MSC_DESC, \ + ... + #define UDI_COMPOSITE_DESC_HS \ + .udi_msc = UDI_MSC_DESC, \ + ... + #define UDI_COMPOSITE_API \ + &udi_api_msc, \ + ... +\endcode + * + * \subsection udi_msc_use_case_composite_usage_flow Workflow + * -# Ensure that conf_usb.h is available and contains the following parameters + * required for a USB composite device configuration: + * - \code // Endpoint control size, This must be: + // - 8, 16, 32 or 64 for full speed device (8 is recommended to save RAM) + // - 64 for a high speed device + #define USB_DEVICE_EP_CTRL_SIZE 64 + // Total Number of interfaces on this USB device. + // Add 1 for MSC. + #define USB_DEVICE_NB_INTERFACE (X+1) + // Total number of endpoints on this USB device. + // This must include each endpoint for each interface. + // Add 2 for MSC. + #define USB_DEVICE_MAX_EP (X+2) \endcode + * -# Ensure that conf_usb.h contains the description of + * composite device: + * - \code // The endpoint numbers chosen by you for the MSC. + // The endpoint numbers starting from 1. + #define UDI_MSC_EP_IN (X | USB_EP_DIR_IN) + #define UDI_MSC_EP_OUT (Y | USB_EP_DIR_OUT) + // The interface index of an interface starting from 0 + #define UDI_MSC_IFACE_NUMBER X \endcode + * -# Ensure that conf_usb.h contains the following parameters + * required for a USB composite device configuration: + * - \code // USB Interfaces descriptor structure + #define UDI_COMPOSITE_DESC_T \ + ... + udi_msc_desc_t udi_msc; \ + ... + // USB Interfaces descriptor value for Full Speed + #define UDI_COMPOSITE_DESC_FS \ + ... + .udi_msc = UDI_MSC_DESC_FS, \ + ... + // USB Interfaces descriptor value for High Speed + #define UDI_COMPOSITE_DESC_HS \ + ... + .udi_msc = UDI_MSC_DESC_HS, \ + ... + // USB Interface APIs + #define UDI_COMPOSITE_API \ + ... + &udi_api_msc, \ + ... \endcode + * - \note The descriptors order given in the four lists above must be the + * same as the order defined by all interface indexes. The interface index + * orders are defined through UDI_X_IFACE_NUMBER defines. + */ + +#endif // _UDI_MSC_H_ diff --git a/src/HAL/DUE/usb/uotghs_device_due.c b/src/HAL/DUE/usb/uotghs_device_due.c new file mode 100644 index 0000000..c7e8f8d --- /dev/null +++ b/src/HAL/DUE/usb/uotghs_device_due.c @@ -0,0 +1,2074 @@ +/** + * \file + * + * \brief USB Device Driver for UOTGHS. Compliant with common UDD driver. + * + * Copyright (c) 2012-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ + +/* + * Support and FAQ: visit Atmel Support + */ + +#ifdef ARDUINO_ARCH_SAM + +#include "compiler.h" +#include "uotghs_device_due.h" + +#include "conf_usb.h" +#include "sysclk.h" +#include "udd.h" +#include "uotghs_otg.h" +#include + +#ifndef UDD_NO_SLEEP_MGR +# include "sleep.h" +# include "sleepmgr.h" +#endif + +#if !(SAM3XA) +# error The current UOTGHS Device Driver supports only SAM3X and SAM3A. +#endif +#ifndef UDD_USB_INT_FUN +# define UDD_USB_INT_FUN UOTGHS_Handler +#endif + +#ifndef UDD_USB_INT_LEVEL +# define UDD_USB_INT_LEVEL 5 // By default USB interrupt have low priority +#endif + +#define UDD_EP_USED(ep) (USB_DEVICE_MAX_EP >= ep) + +#if ( (UDD_EP_USED( 1) && Is_udd_endpoint_dma_supported( 1)) \ + ||(UDD_EP_USED( 2) && Is_udd_endpoint_dma_supported( 2)) \ + ||(UDD_EP_USED( 3) && Is_udd_endpoint_dma_supported( 3)) \ + ||(UDD_EP_USED( 4) && Is_udd_endpoint_dma_supported( 4)) \ + ||(UDD_EP_USED( 5) && Is_udd_endpoint_dma_supported( 5)) \ + ||(UDD_EP_USED( 6) && Is_udd_endpoint_dma_supported( 6)) \ + ||(UDD_EP_USED( 7) && Is_udd_endpoint_dma_supported( 7)) \ + ||(UDD_EP_USED( 8) && Is_udd_endpoint_dma_supported( 8)) \ + ||(UDD_EP_USED( 9) && Is_udd_endpoint_dma_supported( 9)) \ + ||(UDD_EP_USED(10) && Is_udd_endpoint_dma_supported(10)) \ + ||(UDD_EP_USED(11) && Is_udd_endpoint_dma_supported(11)) \ + ||(UDD_EP_USED(12) && Is_udd_endpoint_dma_supported(12)) \ + ||(UDD_EP_USED(13) && Is_udd_endpoint_dma_supported(13)) \ + ||(UDD_EP_USED(14) && Is_udd_endpoint_dma_supported(14)) \ + ||(UDD_EP_USED(15) && Is_udd_endpoint_dma_supported(15)) \ + ) +# define UDD_EP_DMA_SUPPORTED +#endif + +#if ( (UDD_EP_USED( 1) && !Is_udd_endpoint_dma_supported( 1)) \ + ||(UDD_EP_USED( 2) && !Is_udd_endpoint_dma_supported( 2)) \ + ||(UDD_EP_USED( 3) && !Is_udd_endpoint_dma_supported( 3)) \ + ||(UDD_EP_USED( 4) && !Is_udd_endpoint_dma_supported( 4)) \ + ||(UDD_EP_USED( 5) && !Is_udd_endpoint_dma_supported( 5)) \ + ||(UDD_EP_USED( 6) && !Is_udd_endpoint_dma_supported( 6)) \ + ||(UDD_EP_USED( 7) && !Is_udd_endpoint_dma_supported( 7)) \ + ||(UDD_EP_USED( 8) && !Is_udd_endpoint_dma_supported( 8)) \ + ||(UDD_EP_USED( 9) && !Is_udd_endpoint_dma_supported( 9)) \ + ||(UDD_EP_USED(10) && !Is_udd_endpoint_dma_supported(10)) \ + ||(UDD_EP_USED(11) && !Is_udd_endpoint_dma_supported(11)) \ + ||(UDD_EP_USED(12) && !Is_udd_endpoint_dma_supported(12)) \ + ||(UDD_EP_USED(13) && !Is_udd_endpoint_dma_supported(13)) \ + ||(UDD_EP_USED(14) && !Is_udd_endpoint_dma_supported(14)) \ + ||(UDD_EP_USED(15) && !Is_udd_endpoint_dma_supported(15)) \ + ) +# define UDD_EP_FIFO_SUPPORTED +#endif + +// for debug text +//#define dbg_print printf +#define dbg_print(...) + +/** + * \ingroup udd_group + * \defgroup udd_udphs_group USB On-The-Go High-Speed Port for device mode (UOTGHS) + * + * \section UOTGHS_CONF UOTGHS Custom configuration + * The following UOTGHS driver configuration must be included in the conf_usb.h + * file of the application. + * + * UDD_USB_INT_LEVEL
+ * Option to change the interrupt priority (0 to 15) by default 5 (recommended). + * + * UDD_USB_INT_FUN
+ * Option to fit interrupt function to what defined in exception table. + * + * UDD_ISOCHRONOUS_NB_BANK(ep)
+ * Feature to reduce or increase isochronous endpoints buffering (1 to 3). + * Default value 2. + * + * UDD_BULK_NB_BANK(ep)
+ * Feature to reduce or increase bulk endpoints buffering (1 to 2). + * Default value 2. + * + * UDD_INTERRUPT_NB_BANK(ep)
+ * Feature to reduce or increase interrupt endpoints buffering (1 to 2). + * Default value 1. + * + * \section Callbacks management + * The USB driver is fully managed by interrupt and does not request periodique + * task. Thereby, the USB events use callbacks to transfer the information. + * The callbacks are declared in static during compilation or in variable during + * code execution. + * + * Static declarations defined in conf_usb.h: + * - UDC_VBUS_EVENT(bool b_present)
+ * To signal Vbus level change + * - UDC_SUSPEND_EVENT()
+ * Called when USB bus enter in suspend mode + * - UDC_RESUME_EVENT()
+ * Called when USB bus is wakeup + * - UDC_SOF_EVENT()
+ * Called for each received SOF, Note: Each 1ms in HS/FS mode only. + * + * Dynamic callbacks, called "endpoint job" , are registered + * in udd_ep_job_t structure via the following functions: + * - udd_ep_run()
+ * To call it when a transfer is finish + * - udd_ep_wait_stall_clear()
+ * To call it when a endpoint halt is disabled + * + * \section Power mode management + * The Sleep modes authorized : + * - in USB IDLE state, the UOTGHS needs of USB clock and authorizes up to sleep mode WFI. + * - in USB SUSPEND state, the UOTGHS no needs USB clock and authorizes up to sleep mode WAIT. + * @{ + */ + +// Check USB Device configuration +#ifndef USB_DEVICE_EP_CTRL_SIZE +# error USB_DEVICE_EP_CTRL_SIZE not defined +#endif +#ifndef USB_DEVICE_MAX_EP +# error USB_DEVICE_MAX_EP not defined +#endif + +// Note: USB_DEVICE_MAX_EP does not include control endpoint +#if USB_DEVICE_MAX_EP > (UDD_MAX_PEP_NB-1) +# error USB_DEVICE_MAX_EP is too high and not supported by this part +#endif + +#define UDD_EP_ISO_NBANK_ERROR(ep) \ + ( (UDD_ISOCHRONOUS_NB_BANK(ep) < 1) \ + || (UDD_ISOCHRONOUS_NB_BANK(ep) > 3) ) +#define UDD_EP_BULK_NBANK_ERROR(ep) \ + ( (UDD_BULK_NB_BANK(ep) < 1) || (UDD_BULK_NB_BANK(ep) > 2) ) +#define UDD_EP_INT_NBANK_ERROR(ep) \ + ( (UDD_INTERRUPT_NB_BANK(ep) < 1) || (UDD_INTERRUPT_NB_BANK(ep) > 2) ) + +#define UDD_EP_ISO_NB_BANK_ERROR(ep) \ + (UDD_EP_USED(ep) && UDD_EP_ISO_NBANK_ERROR(ep)) +#define UDD_EP_BULK_NB_BANK_ERROR(ep) \ + (UDD_EP_USED(ep) && UDD_EP_ISO_NBANK_ERROR(ep)) +#define UDD_EP_INT_NB_BANK_ERROR(ep) \ + (UDD_EP_USED(ep) && UDD_EP_ISO_NBANK_ERROR(ep)) + +#define UDD_EP_NB_BANK_ERROR(ep, type) \ + (ATPASTE3(UDD_EP_, type, _NB_BANK_ERROR(ep))) + +#define UDD_ISO_NB_BANK_ERROR \ + ( UDD_EP_NB_BANK_ERROR( 1, ISO) \ + || UDD_EP_NB_BANK_ERROR( 2, ISO) \ + || UDD_EP_NB_BANK_ERROR( 3, ISO) \ + || UDD_EP_NB_BANK_ERROR( 4, ISO) \ + || UDD_EP_NB_BANK_ERROR( 5, ISO) \ + || UDD_EP_NB_BANK_ERROR( 6, ISO) \ + || UDD_EP_NB_BANK_ERROR( 7, ISO) \ + || UDD_EP_NB_BANK_ERROR( 8, ISO) \ + || UDD_EP_NB_BANK_ERROR( 9, ISO) \ + || UDD_EP_NB_BANK_ERROR(10, ISO) \ + || UDD_EP_NB_BANK_ERROR(11, ISO) \ + || UDD_EP_NB_BANK_ERROR(12, ISO) \ + || UDD_EP_NB_BANK_ERROR(13, ISO) \ + || UDD_EP_NB_BANK_ERROR(14, ISO) \ + || UDD_EP_NB_BANK_ERROR(15, ISO) ) +#define UDD_BULK_NB_BANK_ERROR \ + ( UDD_EP_NB_BANK_ERROR( 1, BULK) \ + || UDD_EP_NB_BANK_ERROR( 2, BULK) \ + || UDD_EP_NB_BANK_ERROR( 3, BULK) \ + || UDD_EP_NB_BANK_ERROR( 4, BULK) \ + || UDD_EP_NB_BANK_ERROR( 5, BULK) \ + || UDD_EP_NB_BANK_ERROR( 6, BULK) \ + || UDD_EP_NB_BANK_ERROR( 7, BULK) \ + || UDD_EP_NB_BANK_ERROR( 8, BULK) \ + || UDD_EP_NB_BANK_ERROR( 9, BULK) \ + || UDD_EP_NB_BANK_ERROR(10, BULK) \ + || UDD_EP_NB_BANK_ERROR(11, BULK) \ + || UDD_EP_NB_BANK_ERROR(12, BULK) \ + || UDD_EP_NB_BANK_ERROR(13, BULK) \ + || UDD_EP_NB_BANK_ERROR(14, BULK) \ + || UDD_EP_NB_BANK_ERROR(15, BULK) ) +#define UDD_INTERRUPT_NB_BANK_ERROR \ + ( UDD_EP_NB_BANK_ERROR( 1, INT) \ + || UDD_EP_NB_BANK_ERROR( 2, INT) \ + || UDD_EP_NB_BANK_ERROR( 3, INT) \ + || UDD_EP_NB_BANK_ERROR( 4, INT) \ + || UDD_EP_NB_BANK_ERROR( 5, INT) \ + || UDD_EP_NB_BANK_ERROR( 6, INT) \ + || UDD_EP_NB_BANK_ERROR( 7, INT) \ + || UDD_EP_NB_BANK_ERROR( 8, INT) \ + || UDD_EP_NB_BANK_ERROR( 9, INT) \ + || UDD_EP_NB_BANK_ERROR(10, INT) \ + || UDD_EP_NB_BANK_ERROR(11, INT) \ + || UDD_EP_NB_BANK_ERROR(12, INT) \ + || UDD_EP_NB_BANK_ERROR(13, INT) \ + || UDD_EP_NB_BANK_ERROR(14, INT) \ + || UDD_EP_NB_BANK_ERROR(15, INT) ) + +#ifndef UDD_ISOCHRONOUS_NB_BANK +# define UDD_ISOCHRONOUS_NB_BANK(ep) 2 +#else +# if UDD_ISO_NB_BANK_ERROR +# error UDD_ISOCHRONOUS_NB_BANK(ep) must be define within 1 to 3. +# endif +#endif + +#ifndef UDD_BULK_NB_BANK +# define UDD_BULK_NB_BANK(ep) 2 +#else +# if UDD_BULK_NB_BANK_ERROR +# error UDD_BULK_NB_BANK must be define with 1 or 2. +# endif +#endif + +#ifndef UDD_INTERRUPT_NB_BANK +# define UDD_INTERRUPT_NB_BANK(ep) 1 +#else +# if UDD_INTERRUPT_NB_BANK_ERROR +# error UDD_INTERRUPT_NB_BANK must be define with 1 or 2. +# endif +#endif + + +/** + * \name Power management routine. + */ +//@{ + +#ifndef UDD_NO_SLEEP_MGR + +//! Definition of sleep levels +#define UOTGHS_SLEEP_MODE_USB_SUSPEND SLEEPMGR_WAIT_FAST +#define UOTGHS_SLEEP_MODE_USB_IDLE SLEEPMGR_SLEEP_WFI + +//! State of USB line +static bool udd_b_idle; +//! State of sleep manager +static bool udd_b_sleep_initialized = false; + + +/*! \brief Authorize or not the CPU powerdown mode + * + * \param b_enable true to authorize idle mode + */ +static void udd_sleep_mode(bool b_idle) +{ + if (!b_idle && udd_b_idle) { + dbg_print("_S "); + sleepmgr_unlock_mode(UOTGHS_SLEEP_MODE_USB_IDLE); + } + if (b_idle && !udd_b_idle) { + dbg_print("_W "); + sleepmgr_lock_mode(UOTGHS_SLEEP_MODE_USB_IDLE); + } + udd_b_idle = b_idle; +} +#else + +static void udd_sleep_mode(bool b_idle) +{ + b_idle = b_idle; +} + +#endif // UDD_NO_SLEEP_MGR + +//@} + + +/** + * \name Control endpoint low level management routine. + * + * This function performs control endpoint management. + * It handle the SETUP/DATA/HANDSHAKE phases of a control transaction. + */ +//@{ + +//! Global variable to give and record information about setup request management +COMPILER_WORD_ALIGNED udd_ctrl_request_t udd_g_ctrlreq; + +//! Bit definitions about endpoint control state machine for udd_ep_control_state +typedef enum { + UDD_EPCTRL_SETUP = 0, //!< Wait a SETUP packet + UDD_EPCTRL_DATA_OUT = 1, //!< Wait a OUT data packet + UDD_EPCTRL_DATA_IN = 2, //!< Wait a IN data packet + UDD_EPCTRL_HANDSHAKE_WAIT_IN_ZLP = 3, //!< Wait a IN ZLP packet + UDD_EPCTRL_HANDSHAKE_WAIT_OUT_ZLP = 4, //!< Wait a OUT ZLP packet + UDD_EPCTRL_STALL_REQ = 5, //!< STALL enabled on IN & OUT packet +} udd_ctrl_ep_state_t; + +//! State of the endpoint control management +static udd_ctrl_ep_state_t udd_ep_control_state; + +//! Total number of data received/sent during data packet phase with previous payload buffers +static uint16_t udd_ctrl_prev_payload_buf_cnt; + +//! Number of data received/sent to/from udd_g_ctrlreq.payload buffer +static uint16_t udd_ctrl_payload_buf_cnt; + +/** + * \brief Reset control endpoint + * + * Called after a USB line reset or when UDD is enabled + */ +static void udd_reset_ep_ctrl(void); + +/** + * \brief Reset control endpoint management + * + * Called after a USB line reset or at the end of SETUP request (after ZLP) + */ +static void udd_ctrl_init(void); + +//! \brief Managed reception of SETUP packet on control endpoint +static void udd_ctrl_setup_received(void); + +//! \brief Managed reception of IN packet on control endpoint +static void udd_ctrl_in_sent(void); + +//! \brief Managed reception of OUT packet on control endpoint +static void udd_ctrl_out_received(void); + +//! \brief Managed underflow event of IN packet on control endpoint +static void udd_ctrl_underflow(void); + +//! \brief Managed overflow event of OUT packet on control endpoint +static void udd_ctrl_overflow(void); + +//! \brief Managed stall event of IN/OUT packet on control endpoint +static void udd_ctrl_stall_data(void); + +//! \brief Send a ZLP IN on control endpoint +static void udd_ctrl_send_zlp_in(void); + +//! \brief Send a ZLP OUT on control endpoint +static void udd_ctrl_send_zlp_out(void); + +//! \brief Call callback associated to setup request +static void udd_ctrl_endofrequest(void); + + +/** + * \brief Main interrupt routine for control endpoint + * + * This switches control endpoint events to correct sub function. + * + * \return \c 1 if an event about control endpoint is occurred, otherwise \c 0. + */ +static bool udd_ctrl_interrupt(void); + +//@} + + +/** + * \name Management of bulk/interrupt/isochronous endpoints + * + * The UDD manages the data transfer on endpoints: + * - Start data transfer on endpoint with USB Device DMA + * - Send a ZLP packet if requested + * - Call callback registered to signal end of transfer + * The transfer abort and stall feature are supported. + */ +//@{ +#if (0!=USB_DEVICE_MAX_EP) + +//! Structure definition about job registered on an endpoint +typedef struct { + union { + //! Callback to call at the end of transfer + udd_callback_trans_t call_trans; + + //! Callback to call when the endpoint halt is cleared + udd_callback_halt_cleared_t call_nohalt; + }; + //! Buffer located in internal RAM to send or fill during job + uint8_t *buf; + //! Size of buffer to send or fill + iram_size_t buf_size; + //!< Size of data transferred + iram_size_t buf_cnt; + //!< Size of data loaded (or prepared for DMA) last time + iram_size_t buf_load; + //! A job is registered on this endpoint + uint8_t busy:1; + //! A short packet is requested for this job on endpoint IN + uint8_t b_shortpacket:1; + //! A stall has been requested but not executed + uint8_t stall_requested:1; +} udd_ep_job_t; + + +//! Array to register a job on bulk/interrupt/isochronous endpoint +static udd_ep_job_t udd_ep_job[USB_DEVICE_MAX_EP]; + +//! \brief Reset all job table +static void udd_ep_job_table_reset(void); + +//! \brief Abort all endpoint jobs on going +static void udd_ep_job_table_kill(void); + +#ifdef UDD_EP_FIFO_SUPPORTED + /** + * \brief Fill banks and send them + * + * \param ep endpoint number of job to abort + */ + static void udd_ep_in_sent(udd_ep_id_t ep); + + /** + * \brief Store received banks + * + * \param ep endpoint number of job to abort + */ + static void udd_ep_out_received(udd_ep_id_t ep); +#endif + +/** + * \brief Abort endpoint job on going + * + * \param ep endpoint number of job to abort + */ +static void udd_ep_abort_job(udd_ep_id_t ep); + +/** + * \brief Call the callback associated to the job which is finished + * + * \param ptr_job job to complete + * \param b_abort if true then the job has been aborted + */ +static void udd_ep_finish_job(udd_ep_job_t * ptr_job, bool b_abort, uint8_t ep_num); + +#ifdef UDD_EP_DMA_SUPPORTED + /** + * \brief Start the next transfer if necessary or complete the job associated. + * + * \param ep endpoint number without direction flag + */ + static void udd_ep_trans_done(udd_ep_id_t ep); +#endif + +/** + * \brief Main interrupt routine for bulk/interrupt/isochronous endpoints + * + * This switches endpoint events to correct sub function. + * + * \return \c 1 if an event about bulk/interrupt/isochronous endpoints has occurred, otherwise \c 0. + */ +static bool udd_ep_interrupt(void); + +#endif // (0!=USB_DEVICE_MAX_EP) +//@} + + +// ------------------------ +//--- INTERNAL ROUTINES TO MANAGED GLOBAL EVENTS + +/** + * \internal + * \brief Function called by UOTGHS interrupt to manage USB Device interrupts + * + * USB Device interrupt events are splited in three parts: + * - USB line events (SOF, reset, suspend, resume, wakeup) + * - control endpoint events (setup reception, end of data transfer, underflow, overflow, stall) + * - bulk/interrupt/isochronous endpoints events (end of data transfer) + * + * Note: + * Here, the global interrupt mask is not clear when an USB interrupt is enabled + * because this one can not be occurred during the USB ISR (=during INTX is masked). + * See Technical reference $3.8.3 Masking interrupt requests in peripheral modules. + */ +#ifdef UHD_ENABLE +void udd_interrupt(void); +void udd_interrupt(void) +#else +ISR(UDD_USB_INT_FUN) +#endif +{ + /* For fast wakeup clocks restore + * In WAIT mode, clocks are switched to FASTRC. + * After wakeup clocks should be restored, before that ISR should not + * be served. + */ + if (!pmc_is_wakeup_clocks_restored() && !Is_udd_suspend()) { + cpu_irq_disable(); + return; + } + + if (Is_udd_sof()) { + udd_ack_sof(); + if (Is_udd_full_speed_mode()) { + udc_sof_notify(); + } +#ifdef UDC_SOF_EVENT + UDC_SOF_EVENT(); +#endif + goto udd_interrupt_sof_end; + } + + if (Is_udd_msof()) { + udd_ack_msof(); + udc_sof_notify(); + goto udd_interrupt_sof_end; + } + + dbg_print("%c ", udd_is_high_speed() ? 'H' : 'F'); + + if (udd_ctrl_interrupt()) { + goto udd_interrupt_end; // Interrupt acked by control endpoint managed + } + +#if (0 != USB_DEVICE_MAX_EP) + if (udd_ep_interrupt()) { + goto udd_interrupt_end; // Interrupt acked by bulk/interrupt/isochronous endpoint managed + } +#endif + + // USB bus reset detection + if (Is_udd_reset()) { + udd_ack_reset(); + dbg_print("RST "); + // Abort all jobs on-going +#if (USB_DEVICE_MAX_EP != 0) + udd_ep_job_table_kill(); +#endif + // Reset USB Device Stack Core + udc_reset(); + // Reset endpoint control + udd_reset_ep_ctrl(); + // Reset endpoint control management + udd_ctrl_init(); + goto udd_interrupt_end; + } + + if (Is_udd_suspend_interrupt_enabled() && Is_udd_suspend()) { + otg_unfreeze_clock(); + // The suspend interrupt is automatic acked when a wakeup occur + udd_disable_suspend_interrupt(); + udd_enable_wake_up_interrupt(); + otg_freeze_clock(); // Mandatory to exit of sleep mode after a wakeup event + udd_sleep_mode(false); // Enter in SUSPEND mode +#ifdef UDC_SUSPEND_EVENT + UDC_SUSPEND_EVENT(); +#endif + goto udd_interrupt_end; + } + + if (Is_udd_wake_up_interrupt_enabled() && Is_udd_wake_up()) { + // Ack wakeup interrupt and enable suspend interrupt + otg_unfreeze_clock(); + // Check USB clock ready after suspend and eventually sleep USB clock + while (!Is_otg_clock_usable()) { + if (Is_udd_suspend()) { + break; // In case of USB state change in HS + } + }; + // The wakeup interrupt is automatic acked when a suspend occur + udd_disable_wake_up_interrupt(); + udd_enable_suspend_interrupt(); + udd_sleep_mode(true); // Enter in IDLE mode +#ifdef UDC_RESUME_EVENT + UDC_RESUME_EVENT(); +#endif + goto udd_interrupt_end; + } + + if (Is_otg_vbus_transition()) { + dbg_print("VBus "); + // Ack Vbus transition and send status to high level + otg_unfreeze_clock(); + otg_ack_vbus_transition(); + otg_freeze_clock(); +#ifndef USB_DEVICE_ATTACH_AUTO_DISABLE + if (Is_otg_vbus_high()) { + udd_attach(); + } else { + udd_detach(); + } +#endif +#ifdef UDC_VBUS_EVENT + UDC_VBUS_EVENT(Is_otg_vbus_high()); +#endif + goto udd_interrupt_end; + } +udd_interrupt_end: + dbg_print("\n\r"); +udd_interrupt_sof_end: + return; +} + + +bool udd_include_vbus_monitoring(void) +{ + return true; +} + + +void udd_enable(void) +{ + irqflags_t flags; + + flags = cpu_irq_save(); + +#ifdef UHD_ENABLE + // DUAL ROLE INITIALIZATION + if (otg_dual_enable()) { + // The current mode has been started by otg_dual_enable() + cpu_irq_restore(flags); + return; + } +#else + // SINGLE DEVICE MODE INITIALIZATION + pmc_enable_periph_clk(ID_UOTGHS); + sysclk_enable_usb(); + + // Here, only the device mode is possible, then link UOTGHS interrupt to UDD interrupt + NVIC_SetPriority((IRQn_Type) ID_UOTGHS, UDD_USB_INT_LEVEL); + NVIC_EnableIRQ((IRQn_Type) ID_UOTGHS); + + // Always authorize asynchrone USB interrupts to exit of sleep mode + // For SAM USB wake up device except BACKUP mode + pmc_set_fast_startup_input(PMC_FSMR_USBAL); +#endif + +#if (defined USB_ID_GPIO) && (defined UHD_ENABLE) + // Check that the device mode is selected by ID pin + if (!Is_otg_id_device()) { + cpu_irq_restore(flags); + return; // Device is not the current mode + } +#else + // ID pin not used then force device mode + otg_disable_id_pin(); + otg_force_device_mode(); +#endif + // Enable USB hardware + otg_enable_pad(); + otg_enable(); + + // Set the USB speed requested by configuration file +#ifdef USB_DEVICE_LOW_SPEED + udd_low_speed_enable(); +#else + udd_low_speed_disable(); +# ifdef USB_DEVICE_HS_SUPPORT + udd_high_speed_enable(); +# else + udd_high_speed_disable(); +# endif +#endif // USB_DEVICE_LOW_SPEED + + // Check USB clock + otg_unfreeze_clock(); + while (!Is_otg_clock_usable()); + + // Reset internal variables +#if (0!=USB_DEVICE_MAX_EP) + udd_ep_job_table_reset(); +#endif + + otg_ack_vbus_transition(); + // Force Vbus interrupt in case of Vbus always with a high level + // This is possible with a short timing between a Host mode stop/start. + if (Is_otg_vbus_high()) { + otg_raise_vbus_transition(); + } + otg_enable_vbus_interrupt(); + otg_freeze_clock(); + +#ifndef UDD_NO_SLEEP_MGR + if (!udd_b_sleep_initialized) { + udd_b_sleep_initialized = true; + // Initialize the sleep mode authorized for the USB suspend mode + udd_b_idle = false; + sleepmgr_lock_mode(UOTGHS_SLEEP_MODE_USB_SUSPEND); + } else { + udd_sleep_mode(false); // Enter idle mode + } +#endif + + cpu_irq_restore(flags); +} + + +void udd_disable(void) +{ + irqflags_t flags; + +#ifdef UHD_ENABLE +# ifdef USB_ID_GPIO + if (Is_otg_id_host()) { + // Freeze clock to switch mode + otg_freeze_clock(); + udd_detach(); + otg_disable(); + return; // Host mode running, ignore UDD disable + } +# else + if (Is_otg_host_mode_forced()) { + return; // Host mode running, ignore UDD disable + } +# endif +#endif + + flags = cpu_irq_save(); + otg_unfreeze_clock(); + udd_detach(); +#ifndef UDD_NO_SLEEP_MGR + if (udd_b_sleep_initialized) { + udd_b_sleep_initialized = false; + sleepmgr_unlock_mode(UOTGHS_SLEEP_MODE_USB_SUSPEND); + } +#endif + +#ifndef UHD_ENABLE + otg_disable(); + otg_disable_pad(); + sysclk_disable_usb(); + pmc_disable_periph_clk(ID_UOTGHS); + // Else the USB clock disable is done by UHC which manage USB dual role +#endif + cpu_irq_restore(flags); +} + + +void udd_attach(void) +{ + irqflags_t flags; + flags = cpu_irq_save(); + + // At startup the USB bus state is unknown, + // therefore the state is considered IDLE to not miss any USB event + udd_sleep_mode(true); + otg_unfreeze_clock(); + + // This section of clock check can be improved with a check of + // USB clock source via sysclk() + // Check USB clock because the source can be a PLL + while (!Is_otg_clock_usable()); + + // Authorize attach if Vbus is present + udd_attach_device(); + + // Enable USB line events + udd_enable_reset_interrupt(); + udd_enable_suspend_interrupt(); + udd_enable_wake_up_interrupt(); + udd_enable_sof_interrupt(); +#ifdef USB_DEVICE_HS_SUPPORT + udd_enable_msof_interrupt(); +#endif + // Reset following interrupts flag + udd_ack_reset(); + udd_ack_sof(); + udd_ack_msof(); + + // The first suspend interrupt must be forced + // The first suspend interrupt is not detected else raise it + udd_raise_suspend(); + + udd_ack_wake_up(); + otg_freeze_clock(); + cpu_irq_restore(flags); +} + + +void udd_detach(void) +{ + otg_unfreeze_clock(); + + // Detach device from the bus + udd_detach_device(); + otg_freeze_clock(); + udd_sleep_mode(false); +} + + +bool udd_is_high_speed(void) +{ +#ifdef USB_DEVICE_HS_SUPPORT + return !Is_udd_full_speed_mode(); +#else + return false; +#endif +} + + +void udd_set_address(uint8_t address) +{ + udd_disable_address(); + udd_configure_address(address); + udd_enable_address(); +} + + +uint8_t udd_getaddress(void) +{ + return udd_get_configured_address(); +} + + +uint16_t udd_get_frame_number(void) +{ + return udd_frame_number(); +} + +uint16_t udd_get_micro_frame_number(void) +{ + return udd_micro_frame_number(); +} + +void udd_send_remotewakeup(void) +{ +#ifndef UDD_NO_SLEEP_MGR + if (!udd_b_idle) +#endif + { + udd_sleep_mode(true); // Enter in IDLE mode + otg_unfreeze_clock(); + udd_initiate_remote_wake_up(); + } +} + + +void udd_set_setup_payload(uint8_t *payload, uint16_t payload_size) +{ + udd_g_ctrlreq.payload = payload; + udd_g_ctrlreq.payload_size = payload_size; +} + + +#if (0 != USB_DEVICE_MAX_EP) +bool udd_ep_alloc(udd_ep_id_t ep, uint8_t bmAttributes, + uint16_t MaxEndpointSize) +{ + bool b_dir_in; + uint16_t ep_allocated; + uint8_t nb_bank, bank, i; + + b_dir_in = ep & USB_EP_DIR_IN; + ep = ep & USB_EP_ADDR_MASK; + + if (ep > USB_DEVICE_MAX_EP) { + return false; + } + if (Is_udd_endpoint_enabled(ep)) { + return false; + } + dbg_print("alloc(%x, %d) ", ep, MaxEndpointSize); + + // Bank choice + switch (bmAttributes & USB_EP_TYPE_MASK) { + case USB_EP_TYPE_ISOCHRONOUS: + nb_bank = UDD_ISOCHRONOUS_NB_BANK(ep); + break; + case USB_EP_TYPE_INTERRUPT: + nb_bank = UDD_INTERRUPT_NB_BANK(ep); + break; + case USB_EP_TYPE_BULK: + nb_bank = UDD_BULK_NB_BANK(ep); + break; + default: + Assert(false); + return false; + } + switch (nb_bank) { + case 1: + bank = UOTGHS_DEVEPTCFG_EPBK_1_BANK >> + UOTGHS_DEVEPTCFG_EPBK_Pos; + break; + case 2: + bank = UOTGHS_DEVEPTCFG_EPBK_2_BANK >> + UOTGHS_DEVEPTCFG_EPBK_Pos; + break; + case 3: + bank = UOTGHS_DEVEPTCFG_EPBK_3_BANK >> + UOTGHS_DEVEPTCFG_EPBK_Pos; + break; + default: + Assert(false); + return false; + } + + // Check if endpoint size is 8,16,32,64,128,256,512 or 1023 + Assert(MaxEndpointSize < 1024); + Assert((MaxEndpointSize == 1023) + || !(MaxEndpointSize & (MaxEndpointSize - 1))); + Assert(MaxEndpointSize >= 8); + + // Set configuration of new endpoint + udd_configure_endpoint(ep, bmAttributes, (b_dir_in ? 1 : 0), + MaxEndpointSize, bank); + ep_allocated = 1 << ep; + + // Unalloc endpoints superior + for (i = USB_DEVICE_MAX_EP; i > ep; i--) { + if (Is_udd_endpoint_enabled(i)) { + ep_allocated |= 1 << i; + udd_disable_endpoint(i); + udd_unallocate_memory(i); + } + } + + // Realloc/Enable endpoints + for (i = ep; i <= USB_DEVICE_MAX_EP; i++) { + if (ep_allocated & (1 << i)) { + udd_ep_job_t *ptr_job = &udd_ep_job[i - 1]; + bool b_restart = ptr_job->busy; + // Restart running job because + // memory window slides up and its data is lost + ptr_job->busy = false; + // Re-allocate memory + udd_allocate_memory(i); + udd_enable_endpoint(i); + if (!Is_udd_endpoint_configured(i)) { + dbg_print("ErrRealloc%d ", i); + if (NULL == ptr_job->call_trans) { + return false; + } + if (Is_udd_endpoint_in(i)) { + i |= USB_EP_DIR_IN; + } + ptr_job->call_trans(UDD_EP_TRANSFER_ABORT, + ptr_job->buf_cnt, i); + return false; + } + udd_enable_endpoint_bank_autoswitch(i); + if (b_restart) { + // Re-run the job remaining part +# ifdef UDD_EP_FIFO_SUPPORTED + if (!Is_udd_endpoint_dma_supported(i) + && !Is_udd_endpoint_in(i)) { + ptr_job->buf_cnt -= ptr_job->buf_load; + } +# else + ptr_job->buf_cnt -= ptr_job->buf_load; +# endif + b_restart = udd_ep_run(Is_udd_endpoint_in(i) ? + (i | USB_EP_DIR_IN) : i, + ptr_job->b_shortpacket, + &ptr_job->buf[ptr_job->buf_cnt], + ptr_job->buf_size + - ptr_job->buf_cnt, + ptr_job->call_trans); + if (!b_restart) { + dbg_print("ErrReRun%d ", i); + return false; + } + } + } + } + return true; +} + + +void udd_ep_free(udd_ep_id_t ep) +{ + uint8_t ep_index = ep & USB_EP_ADDR_MASK; + if (USB_DEVICE_MAX_EP < ep_index) { + return; + } + udd_disable_endpoint(ep_index); + udd_unallocate_memory(ep_index); + udd_ep_abort_job(ep); + udd_ep_job[ep_index - 1].stall_requested = false; +} + + +bool udd_ep_is_halted(udd_ep_id_t ep) +{ + uint8_t ep_index = ep & USB_EP_ADDR_MASK; + return Is_udd_endpoint_stall_requested(ep_index); +} + + +bool udd_ep_set_halt(udd_ep_id_t ep) +{ + uint8_t ep_index = ep & USB_EP_ADDR_MASK; + udd_ep_job_t *ptr_job = &udd_ep_job[ep_index - 1]; + irqflags_t flags; + + if (USB_DEVICE_MAX_EP < ep_index) { + return false; + } + + if (Is_udd_endpoint_stall_requested(ep_index) // Endpoint stalled + || ptr_job->stall_requested) { // Endpoint stall is requested + return true; // Already STALL + } + + if (ptr_job->busy == true) { + return false; // Job on going, stall impossible + } + + flags = cpu_irq_save(); + if ((ep & USB_EP_DIR_IN) && (0 != udd_nb_busy_bank(ep_index))) { + // Delay the stall after the end of IN transfer on USB line + ptr_job->stall_requested = true; +#ifdef UDD_EP_FIFO_SUPPORTED + udd_disable_in_send_interrupt(ep_index); + udd_enable_endpoint_bank_autoswitch(ep_index); +#endif + udd_enable_bank_interrupt(ep_index); + udd_enable_endpoint_interrupt(ep_index); + cpu_irq_restore(flags); + return true; + } + // Stall endpoint immediately + udd_disable_endpoint_bank_autoswitch(ep_index); + udd_ack_stall(ep_index); + udd_enable_stall_handshake(ep_index); + cpu_irq_restore(flags); + return true; +} + + +bool udd_ep_clear_halt(udd_ep_id_t ep) +{ + uint8_t ep_index = ep & USB_EP_ADDR_MASK; + udd_ep_job_t *ptr_job = &udd_ep_job[ep_index - 1]; + bool b_stall_cleared = false; + + if (USB_DEVICE_MAX_EP < ep_index) + return false; + + if (ptr_job->stall_requested) { + // Endpoint stall has been requested but not done + // Remove stall request + ptr_job->stall_requested = false; + udd_disable_bank_interrupt(ep_index); + udd_disable_endpoint_interrupt(ep_index); + b_stall_cleared = true; + } + if (Is_udd_endpoint_stall_requested(ep_index)) { + if (Is_udd_stall(ep_index)) { + udd_ack_stall(ep_index); + // A packet has been stalled + // then reset datatoggle + udd_reset_data_toggle(ep_index); + } + // Disable stall + udd_disable_stall_handshake(ep_index); + udd_enable_endpoint_bank_autoswitch(ep_index); + b_stall_cleared = true; + } + if (b_stall_cleared) { + // If a job is register on clear halt action + // then execute callback + if (ptr_job->busy == true) { + ptr_job->busy = false; + ptr_job->call_nohalt(); + } + } + return true; +} + + +bool udd_ep_run(udd_ep_id_t ep, bool b_shortpacket, + uint8_t * buf, iram_size_t buf_size, + udd_callback_trans_t callback) +{ +#ifdef UDD_EP_FIFO_SUPPORTED + bool b_dir_in = Is_udd_endpoint_in(ep & USB_EP_ADDR_MASK); +#endif + udd_ep_job_t *ptr_job; + irqflags_t flags; + + ep &= USB_EP_ADDR_MASK; + if (USB_DEVICE_MAX_EP < ep) { + return false; + } + + // Get job about endpoint + ptr_job = &udd_ep_job[ep - 1]; + + if ((!Is_udd_endpoint_enabled(ep)) + || Is_udd_endpoint_stall_requested(ep) + || ptr_job->stall_requested) { + return false; // Endpoint is halted + } + + flags = cpu_irq_save(); + if (ptr_job->busy == true) { + cpu_irq_restore(flags); + return false; // Job already on going + } + ptr_job->busy = true; + cpu_irq_restore(flags); + + // No job running. Let's setup a new one. + ptr_job->buf = buf; + ptr_job->buf_size = buf_size; + ptr_job->buf_cnt = 0; + ptr_job->buf_load = 0; + ptr_job->call_trans = callback; + ptr_job->b_shortpacket = b_shortpacket || (buf_size == 0); + +#ifdef UDD_EP_FIFO_SUPPORTED + // No DMA support + if (!Is_udd_endpoint_dma_supported(ep)) { + dbg_print("ex%x.%c%d\n\r", ep, b_dir_in ? 'i':'o', buf_size); + flags = cpu_irq_save(); + udd_enable_endpoint_interrupt(ep); + if (b_dir_in) { + udd_disable_endpoint_bank_autoswitch(ep); + udd_enable_in_send_interrupt(ep); + } else { + udd_disable_endpoint_bank_autoswitch(ep); + udd_enable_out_received_interrupt(ep); + } + cpu_irq_restore(flags); + return true; + } +#endif // UDD_EP_FIFO_SUPPORTED + +#ifdef UDD_EP_DMA_SUPPORTED + // Request first DMA transfer + dbg_print("(exDMA%x) ", ep); + udd_ep_trans_done(ep); + return true; +#endif +} + + +void udd_ep_abort(udd_ep_id_t ep) +{ + uint8_t ep_index = ep & USB_EP_ADDR_MASK; + +#ifdef UDD_EP_FIFO_SUPPORTED + if (!Is_udd_endpoint_dma_supported(ep_index)) { + // Disable interrupts + udd_disable_endpoint_interrupt(ep_index); + udd_disable_out_received_interrupt(ep_index); + udd_disable_in_send_interrupt(ep_index); + } else +#endif + { + // Stop DMA transfer + udd_disable_endpoint_dma_interrupt(ep_index); + udd_endpoint_dma_set_control(ep_index, 0); + } + udd_disable_endpoint_interrupt(ep_index); + // Kill IN banks + if (ep & USB_EP_DIR_IN) { + while(udd_nb_busy_bank(ep_index)) { + udd_kill_last_in_bank(ep_index); + while(Is_udd_kill_last(ep_index)); + } + } + udd_ep_abort_job(ep); +} + + +bool udd_ep_wait_stall_clear(udd_ep_id_t ep, + udd_callback_halt_cleared_t callback) +{ + udd_ep_job_t *ptr_job; + + ep &= USB_EP_ADDR_MASK; + if (USB_DEVICE_MAX_EP < ep) { + return false; + } + + ptr_job = &udd_ep_job[ep - 1]; + + if (!Is_udd_endpoint_enabled(ep)) { + return false; // Endpoint not enabled + } + + // Wait clear halt endpoint + if (ptr_job->busy == true) { + return false; // Job already on going + } + + if (Is_udd_endpoint_stall_requested(ep) + || ptr_job->stall_requested) { + // Endpoint halted then registers the callback + ptr_job->busy = true; + ptr_job->call_nohalt = callback; + } else { + // endpoint not halted then call directly callback + callback(); + } + return true; +} +#endif // (0 != USB_DEVICE_MAX_EP) + + +#ifdef USB_DEVICE_HS_SUPPORT + +void udd_test_mode_j(void) +{ + udd_enable_hs_test_mode(); + udd_enable_hs_test_mode_j(); +} + + +void udd_test_mode_k(void) +{ + udd_enable_hs_test_mode(); + udd_enable_hs_test_mode_k(); +} + + +void udd_test_mode_se0_nak(void) +{ + udd_enable_hs_test_mode(); +} + + +void udd_test_mode_packet(void) +{ + uint8_t i; + uint8_t *ptr_dest; + const uint8_t *ptr_src; + + const uint8_t test_packet[] = { + // 00000000 * 9 + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + // 01010101 * 8 + 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, + // 01110111 * 8 + 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, + // 0, {111111S * 15}, 111111 + 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, + // S, 111111S, {0111111S * 7} + 0x7F, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB, 0xFD, + // 00111111, {S0111111 * 9}, S0 + 0xFC, 0x7E, 0xBF, 0xDF, 0xEF, 0xF7, 0xFB, 0xFD, 0x7E + }; + + // Reconfigure control endpoint to bulk IN endpoint + udd_disable_endpoint(0); + udd_configure_endpoint(0, USB_EP_TYPE_BULK, 1, + 64, UOTGHS_DEVEPTCFG_EPBK_1_BANK); + udd_allocate_memory(0); + udd_enable_endpoint(0); + + udd_enable_hs_test_mode(); + udd_enable_hs_test_mode_packet(); + + // Send packet on endpoint 0 + ptr_dest = (uint8_t *) & udd_get_endpoint_fifo_access(0, 8); + ptr_src = test_packet; + + for (i = 0; i < sizeof(test_packet); i++) { + *ptr_dest++ = *ptr_src++; + } + udd_ack_fifocon(0); +} +#endif // USB_DEVICE_HS_SUPPORT + + + +// ------------------------ +//--- INTERNAL ROUTINES TO MANAGED THE CONTROL ENDPOINT + +static void udd_reset_ep_ctrl(void) +{ + irqflags_t flags; + + // Reset USB address to 0 + udd_configure_address(0); + udd_enable_address(); + + // Alloc and configure control endpoint + udd_configure_endpoint(0, + USB_EP_TYPE_CONTROL, + 0, + USB_DEVICE_EP_CTRL_SIZE, + UOTGHS_DEVEPTCFG_EPBK_1_BANK); + + udd_allocate_memory(0); + udd_enable_endpoint(0); + flags = cpu_irq_save(); + udd_enable_setup_received_interrupt(0); + udd_enable_out_received_interrupt(0); + udd_enable_endpoint_interrupt(0); + cpu_irq_restore(flags); +} + +static void udd_ctrl_init(void) +{ + irqflags_t flags; + flags = cpu_irq_save(); + + // In case of abort of IN Data Phase: + // No need to abort IN transfer (rise TXINI), + // because it is automatically done by hardware when a Setup packet is received. + // But the interrupt must be disabled to don't generate interrupt TXINI + // after SETUP reception. + udd_disable_in_send_interrupt(0); + cpu_irq_restore(flags); + + // In case of OUT ZLP event is no processed before Setup event occurs + udd_ack_out_received(0); + + udd_g_ctrlreq.callback = NULL; + udd_g_ctrlreq.over_under_run = NULL; + udd_g_ctrlreq.payload_size = 0; + udd_ep_control_state = UDD_EPCTRL_SETUP; +} + + +static void udd_ctrl_setup_received(void) +{ + irqflags_t flags; + uint8_t i; + + if (UDD_EPCTRL_SETUP != udd_ep_control_state) { + // May be a hidden DATA or ZLP phase or protocol abort + udd_ctrl_endofrequest(); + + // Reinitializes control endpoint management + udd_ctrl_init(); + } + // Fill setup request structure + if (8 != udd_byte_count(0)) { + udd_ctrl_stall_data(); + udd_ack_setup_received(0); + return; // Error data number doesn't correspond to SETUP packet + } + uint8_t *ptr = (uint8_t *) & udd_get_endpoint_fifo_access(0,8); + for (i = 0; i < 8; i++) { + ((uint8_t*) &udd_g_ctrlreq.req)[i] = *ptr++; + } + // Manage LSB/MSB to fit with CPU usage + udd_g_ctrlreq.req.wValue = le16_to_cpu(udd_g_ctrlreq.req.wValue); + udd_g_ctrlreq.req.wIndex = le16_to_cpu(udd_g_ctrlreq.req.wIndex); + udd_g_ctrlreq.req.wLength = le16_to_cpu(udd_g_ctrlreq.req.wLength); + + // Decode setup request + if (udc_process_setup() == false) { + // Setup request unknown then stall it + udd_ctrl_stall_data(); + udd_ack_setup_received(0); + return; + } + udd_ack_setup_received(0); + + if (Udd_setup_is_in()) { + // IN data phase requested + udd_ctrl_prev_payload_buf_cnt = 0; + udd_ctrl_payload_buf_cnt = 0; + udd_ep_control_state = UDD_EPCTRL_DATA_IN; + udd_ctrl_in_sent(); // Send first data transfer + } else { + if (0 == udd_g_ctrlreq.req.wLength) { + // No data phase requested + // Send IN ZLP to ACK setup request + udd_ctrl_send_zlp_in(); + return; + } + // OUT data phase requested + udd_ctrl_prev_payload_buf_cnt = 0; + udd_ctrl_payload_buf_cnt = 0; + udd_ep_control_state = UDD_EPCTRL_DATA_OUT; + // To detect a protocol error, enable nak interrupt on data IN phase + udd_ack_nak_in(0); + flags = cpu_irq_save(); + udd_enable_nak_in_interrupt(0); + cpu_irq_restore(flags); + } +} + + +static void udd_ctrl_in_sent(void) +{ + static bool b_shortpacket = false; + uint16_t nb_remain; + uint8_t i; + uint8_t *ptr_dest, *ptr_src; + irqflags_t flags; + + flags = cpu_irq_save(); + udd_disable_in_send_interrupt(0); + cpu_irq_restore(flags); + + if (UDD_EPCTRL_HANDSHAKE_WAIT_IN_ZLP == udd_ep_control_state) { + // ZLP on IN is sent, then valid end of setup request + udd_ctrl_endofrequest(); + // Reinitializes control endpoint management + udd_ctrl_init(); + return; + } + Assert(udd_ep_control_state == UDD_EPCTRL_DATA_IN); + + nb_remain = udd_g_ctrlreq.payload_size - udd_ctrl_payload_buf_cnt; + if (0 == nb_remain) { + // All content of current buffer payload are sent + // Update number of total data sending by previous playlaod buffer + udd_ctrl_prev_payload_buf_cnt += udd_ctrl_payload_buf_cnt; + if ((udd_g_ctrlreq.req.wLength == udd_ctrl_prev_payload_buf_cnt) + || b_shortpacket) { + // All data requested are transferred or a short packet has been sent + // then it is the end of data phase. + // Generate an OUT ZLP for handshake phase. + udd_ctrl_send_zlp_out(); + return; + } + // Need of new buffer because the data phase is not complete + if ((!udd_g_ctrlreq.over_under_run) + || (!udd_g_ctrlreq.over_under_run())) { + // Underrun then send zlp on IN + // Here nb_remain=0 and allows to send a IN ZLP + } else { + // A new payload buffer is given + udd_ctrl_payload_buf_cnt = 0; + nb_remain = udd_g_ctrlreq.payload_size; + } + } + // Continue transfer and send next data + if (nb_remain >= USB_DEVICE_EP_CTRL_SIZE) { + nb_remain = USB_DEVICE_EP_CTRL_SIZE; + b_shortpacket = false; + } else { + b_shortpacket = true; + } + // Fill buffer of endpoint control + ptr_dest = (uint8_t *) & udd_get_endpoint_fifo_access(0, 8); + ptr_src = udd_g_ctrlreq.payload + udd_ctrl_payload_buf_cnt; + // Critical section + // Only in case of DATA IN phase abort without USB Reset signal after. + // The IN data don't must be written in endpoint 0 DPRAM during + // a next setup reception in same endpoint 0 DPRAM. + // Thereby, an OUT ZLP reception must check before IN data write + // and if no OUT ZLP is received the data must be written quickly (800µs) + // before an eventually ZLP OUT and SETUP reception + flags = cpu_irq_save(); + if (Is_udd_out_received(0)) { + // IN DATA phase aborted by OUT ZLP + cpu_irq_restore(flags); + udd_ep_control_state = UDD_EPCTRL_HANDSHAKE_WAIT_OUT_ZLP; + return; // Exit of IN DATA phase + } + // Write quickly the IN data + for (i = 0; i < nb_remain; i++) { + *ptr_dest++ = *ptr_src++; + } + udd_ctrl_payload_buf_cnt += nb_remain; + + // Validate and send the data available in the control endpoint buffer + udd_ack_in_send(0); + udd_enable_in_send_interrupt(0); + // In case of abort of DATA IN phase, no need to enable nak OUT interrupt + // because OUT endpoint is already free and ZLP OUT accepted. + cpu_irq_restore(flags); +} + + +static void udd_ctrl_out_received(void) +{ + irqflags_t flags; + uint8_t i; + uint16_t nb_data; + + if (UDD_EPCTRL_DATA_OUT != udd_ep_control_state) { + if ((UDD_EPCTRL_DATA_IN == udd_ep_control_state) + || (UDD_EPCTRL_HANDSHAKE_WAIT_OUT_ZLP == + udd_ep_control_state)) { + // End of SETUP request: + // - Data IN Phase aborted, + // - or last Data IN Phase hidden by ZLP OUT sending quiclky, + // - or ZLP OUT received normally. + udd_ctrl_endofrequest(); + } else { + // Protocol error during SETUP request + udd_ctrl_stall_data(); + } + // Reinitializes control endpoint management + udd_ctrl_init(); + return; + } + // Read data received during OUT phase + nb_data = udd_byte_count(0); + if (udd_g_ctrlreq.payload_size < (udd_ctrl_payload_buf_cnt + nb_data)) { + // Payload buffer too small + nb_data = udd_g_ctrlreq.payload_size - udd_ctrl_payload_buf_cnt; + } + uint8_t *ptr_src = (uint8_t *) & udd_get_endpoint_fifo_access(0, 8); + uint8_t *ptr_dest = udd_g_ctrlreq.payload + udd_ctrl_payload_buf_cnt; + for (i = 0; i < nb_data; i++) { + *ptr_dest++ = *ptr_src++; + } + udd_ctrl_payload_buf_cnt += nb_data; + + if ((USB_DEVICE_EP_CTRL_SIZE != nb_data) + || (udd_g_ctrlreq.req.wLength <= + (udd_ctrl_prev_payload_buf_cnt + + udd_ctrl_payload_buf_cnt))) { + // End of reception because it is a short packet + // Before send ZLP, call intermediate callback + // in case of data receiv generate a stall + udd_g_ctrlreq.payload_size = udd_ctrl_payload_buf_cnt; + if (NULL != udd_g_ctrlreq.over_under_run) { + if (!udd_g_ctrlreq.over_under_run()) { + // Stall ZLP + udd_ctrl_stall_data(); + // Ack reception of OUT to replace NAK by a STALL + udd_ack_out_received(0); + return; + } + } + // Send IN ZLP to ACK setup request + udd_ack_out_received(0); + udd_ctrl_send_zlp_in(); + return; + } + + if (udd_g_ctrlreq.payload_size == udd_ctrl_payload_buf_cnt) { + // Overrun then request a new payload buffer + if (!udd_g_ctrlreq.over_under_run) { + // No callback available to request a new payload buffer + udd_ctrl_stall_data(); + // Ack reception of OUT to replace NAK by a STALL + udd_ack_out_received(0); + return; + } + if (!udd_g_ctrlreq.over_under_run()) { + // No new payload buffer delivered + udd_ctrl_stall_data(); + // Ack reception of OUT to replace NAK by a STALL + udd_ack_out_received(0); + return; + } + // New payload buffer available + // Update number of total data received + udd_ctrl_prev_payload_buf_cnt += udd_ctrl_payload_buf_cnt; + // Reinit reception on payload buffer + udd_ctrl_payload_buf_cnt = 0; + } + // Free buffer of control endpoint to authorize next reception + udd_ack_out_received(0); + // To detect a protocol error, enable nak interrupt on data IN phase + udd_ack_nak_in(0); + flags = cpu_irq_save(); + udd_enable_nak_in_interrupt(0); + cpu_irq_restore(flags); +} + + +static void udd_ctrl_underflow(void) +{ + if (Is_udd_out_received(0)) + return; // Underflow ignored if OUT data is received + + if (UDD_EPCTRL_DATA_OUT == udd_ep_control_state) { + // Host want to stop OUT transaction + // then stop to wait OUT data phase and wait IN ZLP handshake + udd_ctrl_send_zlp_in(); + } else if (UDD_EPCTRL_HANDSHAKE_WAIT_OUT_ZLP == udd_ep_control_state) { + // A OUT handshake is waiting by device, + // but host want extra IN data then stall extra IN data + udd_enable_stall_handshake(0); + } +} + + +static void udd_ctrl_overflow(void) +{ + if (Is_udd_in_send(0)) + return; // Overflow ignored if IN data is received + + // The case of UDD_EPCTRL_DATA_IN is not managed + // because the OUT endpoint is already free and OUT ZLP accepted + + if (UDD_EPCTRL_HANDSHAKE_WAIT_IN_ZLP == udd_ep_control_state) { + // A IN handshake is waiting by device, + // but host want extra OUT data then stall extra OUT data + udd_enable_stall_handshake(0); + } +} + + +static void udd_ctrl_stall_data(void) +{ + // Stall all packets on IN & OUT control endpoint + udd_ep_control_state = UDD_EPCTRL_STALL_REQ; + udd_enable_stall_handshake(0); +} + + +static void udd_ctrl_send_zlp_in(void) +{ + irqflags_t flags; + + udd_ep_control_state = UDD_EPCTRL_HANDSHAKE_WAIT_IN_ZLP; + + // Validate and send empty IN packet on control endpoint + flags = cpu_irq_save(); + // Send ZLP on IN endpoint + udd_ack_in_send(0); + udd_enable_in_send_interrupt(0); + // To detect a protocol error, enable nak interrupt on data OUT phase + udd_ack_nak_out(0); + udd_enable_nak_out_interrupt(0); + cpu_irq_restore(flags); +} + + +static void udd_ctrl_send_zlp_out(void) +{ + irqflags_t flags; + + udd_ep_control_state = UDD_EPCTRL_HANDSHAKE_WAIT_OUT_ZLP; + // No action is necessary to accept OUT ZLP + // because the buffer of control endpoint is already free + + // To detect a protocol error, enable nak interrupt on data IN phase + flags = cpu_irq_save(); + udd_ack_nak_in(0); + udd_enable_nak_in_interrupt(0); + cpu_irq_restore(flags); +} + + +static void udd_ctrl_endofrequest(void) +{ + // If a callback is registered then call it + if (udd_g_ctrlreq.callback) { + udd_g_ctrlreq.callback(); + } +} + + +static bool udd_ctrl_interrupt(void) +{ + + if (!Is_udd_endpoint_interrupt(0)) { + return false; // No interrupt events on control endpoint + } + + dbg_print("0: "); + + // By default disable overflow and underflow interrupt + udd_disable_nak_in_interrupt(0); + udd_disable_nak_out_interrupt(0); + + // Search event on control endpoint + if (Is_udd_setup_received(0)) { + dbg_print("stup "); + // SETUP packet received + udd_ctrl_setup_received(); + return true; + } + if (Is_udd_in_send(0) && Is_udd_in_send_interrupt_enabled(0)) { + dbg_print("in "); + // IN packet sent + udd_ctrl_in_sent(); + return true; + } + if (Is_udd_out_received(0)) { + dbg_print("out "); + // OUT packet received + udd_ctrl_out_received(); + return true; + } + if (Is_udd_nak_out(0)) { + dbg_print("nako "); + // Overflow on OUT packet + udd_ack_nak_out(0); + udd_ctrl_overflow(); + return true; + } + if (Is_udd_nak_in(0)) { + dbg_print("naki "); + // Underflow on IN packet + udd_ack_nak_in(0); + udd_ctrl_underflow(); + return true; + } + dbg_print("n%x ", UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], 0)); + return false; +} + + +// ------------------------ +//--- INTERNAL ROUTINES TO MANAGED THE BULK/INTERRUPT/ISOCHRONOUS ENDPOINTS + +#if (0 != USB_DEVICE_MAX_EP) + +static void udd_ep_job_table_reset(void) +{ + uint8_t i; + for (i = 0; i < USB_DEVICE_MAX_EP; i++) { + udd_ep_job[i].busy = false; + udd_ep_job[i].stall_requested = false; + } +} + + +static void udd_ep_job_table_kill(void) +{ + uint8_t i; + + // For each endpoint, kill job + for (i = 0; i < USB_DEVICE_MAX_EP; i++) { + udd_ep_finish_job(&udd_ep_job[i], true, i + 1); + } +} + + +static void udd_ep_abort_job(udd_ep_id_t ep) +{ + ep &= USB_EP_ADDR_MASK; + + // Abort job on endpoint + udd_ep_finish_job(&udd_ep_job[ep - 1], true, ep); +} + + +static void udd_ep_finish_job(udd_ep_job_t * ptr_job, bool b_abort, uint8_t ep_num) +{ + if (ptr_job->busy == false) { + return; // No on-going job + } + dbg_print("(JobE%x:%d) ", (ptr_job-udd_ep_job)+1, b_abort); + ptr_job->busy = false; + if (NULL == ptr_job->call_trans) { + return; // No callback linked to job + } + if (Is_udd_endpoint_in(ep_num)) { + ep_num |= USB_EP_DIR_IN; + } + ptr_job->call_trans((b_abort) ? UDD_EP_TRANSFER_ABORT : + UDD_EP_TRANSFER_OK, ptr_job->buf_size, ep_num); +} + +#ifdef UDD_EP_DMA_SUPPORTED +static void udd_ep_trans_done(udd_ep_id_t ep) +{ + uint32_t udd_dma_ctrl = 0; + udd_ep_job_t *ptr_job; + iram_size_t next_trans; + irqflags_t flags; + + // Get job corresponding at endpoint + ptr_job = &udd_ep_job[ep - 1]; + + if (!ptr_job->busy) { + return; // No job is running, then ignore it (system error) + } + + if (ptr_job->buf_cnt != ptr_job->buf_size) { + // Need to send or receiv other data + next_trans = ptr_job->buf_size - ptr_job->buf_cnt; + + if (UDD_ENDPOINT_MAX_TRANS < next_trans) { + // The USB hardware support a maximum + // transfer size of UDD_ENDPOINT_MAX_TRANS Bytes + next_trans = UDD_ENDPOINT_MAX_TRANS; + + // Set 0 to transfer the maximum + udd_dma_ctrl = UOTGHS_DEVDMACONTROL_BUFF_LENGTH(0); + } else { + udd_dma_ctrl = UOTGHS_DEVDMACONTROL_BUFF_LENGTH(next_trans); + } + if (Is_udd_endpoint_in(ep)) { + if (0 != (next_trans % udd_get_endpoint_size(ep))) { + // Enable short packet option + // else the DMA transfer is accepted + // and interrupt DMA valid but nothing is sent. + udd_dma_ctrl |= UOTGHS_DEVDMACONTROL_END_B_EN; + // No need to request another ZLP + ptr_job->b_shortpacket = false; + } + } else { + if ((USB_EP_TYPE_ISOCHRONOUS != udd_get_endpoint_type(ep)) + || (next_trans <= (iram_size_t) udd_get_endpoint_size(ep))) { + + // Enable short packet reception + udd_dma_ctrl |= UOTGHS_DEVDMACONTROL_END_TR_IT + | UOTGHS_DEVDMACONTROL_END_TR_EN; + } + } + + // Start USB DMA to fill or read fifo of the selected endpoint + udd_endpoint_dma_set_addr(ep, (uint32_t) & ptr_job->buf[ptr_job->buf_cnt]); + udd_dma_ctrl |= UOTGHS_DEVDMACONTROL_END_BUFFIT | + UOTGHS_DEVDMACONTROL_CHANN_ENB; + + + // Disable IRQs to have a short sequence + // between read of EOT_STA and DMA enable + flags = cpu_irq_save(); + if (!(udd_endpoint_dma_get_status(ep) + & UOTGHS_DEVDMASTATUS_END_TR_ST)) { + dbg_print("dmaS%x ", ep); + udd_endpoint_dma_set_control(ep, udd_dma_ctrl); + ptr_job->buf_cnt += next_trans; + ptr_job->buf_load = next_trans; + udd_enable_endpoint_dma_interrupt(ep); + cpu_irq_restore(flags); + return; + } + cpu_irq_restore(flags); + + // Here a ZLP has been received + // and the DMA transfer must be not started. + // It is the end of transfer + ptr_job->buf_size = ptr_job->buf_cnt; + } + if (Is_udd_endpoint_in(ep)) { + if (ptr_job->b_shortpacket) { + dbg_print("zlpS%x ", ep); + // Need to send a ZLP (No possible with USB DMA) + // enable interrupt to wait a free bank to sent ZLP + udd_ack_in_send(ep); + if (Is_udd_write_enabled(ep)) { + // Force interrupt in case of ep already free + udd_raise_in_send(ep); + } + udd_enable_in_send_interrupt(ep); + udd_enable_endpoint_interrupt(ep); + return; + } + } + dbg_print("dmaE "); + // Call callback to signal end of transfer + udd_ep_finish_job(ptr_job, false, ep); +} +#endif + +#ifdef UDD_EP_FIFO_SUPPORTED +static void udd_ep_in_sent(udd_ep_id_t ep) +{ + udd_ep_job_t *ptr_job = &udd_ep_job[ep - 1]; + uint8_t *ptr_src = &ptr_job->buf[ptr_job->buf_cnt]; + uint8_t *ptr_dst = (uint8_t *) & udd_get_endpoint_fifo_access(ep, 8); + uint32_t pkt_size = udd_get_endpoint_size(ep); + uint32_t nb_data = 0, i; + uint32_t nb_remain; + irqflags_t flags; + + // All transfer done, including ZLP, Finish Job + if (ptr_job->buf_cnt >= ptr_job->buf_size && !ptr_job->b_shortpacket) { + flags = cpu_irq_save(); + udd_disable_in_send_interrupt(ep); + udd_disable_endpoint_interrupt(ep); + cpu_irq_restore(flags); + + ptr_job->buf_size = ptr_job->buf_cnt; // buf_size is passed to callback as XFR count + udd_ep_finish_job(ptr_job, false, ep); + return; + } else { + // ACK TXINI + udd_ack_in_send(ep); + // Fill FIFO + ptr_dst = (uint8_t *) & udd_get_endpoint_fifo_access(ep, 8); + ptr_src = &ptr_job->buf[ptr_job->buf_cnt]; + nb_remain = ptr_job->buf_size - ptr_job->buf_cnt; + // Fill a bank even if no data (ZLP) + nb_data = min(nb_remain, pkt_size); + // Modify job information + ptr_job->buf_cnt += nb_data; + ptr_job->buf_load = nb_data; + + // Copy buffer to FIFO + for (i = 0; i < nb_data; i++) { + *ptr_dst++ = *ptr_src++; + } + // Switch to next bank + udd_ack_fifocon(ep); + // ZLP? + if (nb_data < pkt_size) { + ptr_job->b_shortpacket = false; + } + } +} + +static void udd_ep_out_received(udd_ep_id_t ep) +{ + udd_ep_job_t *ptr_job = &udd_ep_job[ep - 1]; + uint32_t nb_data = 0, i; + uint32_t nb_remain = ptr_job->buf_size - ptr_job->buf_cnt; + uint32_t pkt_size = udd_get_endpoint_size(ep); + uint8_t *ptr_src = (uint8_t *) & udd_get_endpoint_fifo_access(ep, 8); + uint8_t *ptr_dst = &ptr_job->buf[ptr_job->buf_cnt]; + bool b_full = false, b_short = false; + + // Clear RX OUT + udd_ack_out_received(ep); + + // Read byte count + nb_data = udd_byte_count(ep); + if (nb_data < pkt_size) { + b_short = true; + } + //dbg_print("o%d ", ep); + //dbg_print("%d ", nb_data); + // Copy data if there is + if (nb_data > 0) { + if (nb_data >= nb_remain) { + nb_data = nb_remain; + b_full = true; + } + // Modify job information + ptr_job->buf_cnt += nb_data; + ptr_job->buf_load = nb_data; + // Copy FIFO to buffer + for (i = 0; i < nb_data; i++) { + *ptr_dst++ = *ptr_src++; + } + } + // Clear FIFO Status + udd_ack_fifocon(ep); + // Finish job on error or short packet + if (b_full || b_short) { + //dbg_print("EoO%d\n\r", ep); + udd_disable_out_received_interrupt(ep); + udd_disable_endpoint_interrupt(ep); + ptr_job->buf_size = ptr_job->buf_cnt; // buf_size is passed to callback as XFR count + udd_ep_finish_job(ptr_job, false, ep); + } +} +#endif // #ifdef UDD_EP_FIFO_SUPPORTED + +static bool udd_ep_interrupt(void) +{ + udd_ep_id_t ep; + udd_ep_job_t *ptr_job; + + // For each endpoint different of control endpoint (0) + for (ep = 1; ep <= USB_DEVICE_MAX_EP; ep++) { + // Get job corresponding at endpoint + ptr_job = &udd_ep_job[ep - 1]; + +#ifdef UDD_EP_DMA_SUPPORTED + // Check DMA event + if (Is_udd_endpoint_dma_interrupt_enabled(ep) + && Is_udd_endpoint_dma_interrupt(ep)) { + uint32_t nb_remaining; + if (udd_endpoint_dma_get_status(ep) + & UOTGHS_DEVDMASTATUS_CHANN_ENB) { + return true; // Ignore EOT_STA interrupt + } + dbg_print("dma%x: ", ep); + udd_disable_endpoint_dma_interrupt(ep); + // Save number of data no transferred + nb_remaining = (udd_endpoint_dma_get_status(ep) & + UOTGHS_DEVDMASTATUS_BUFF_COUNT_Msk) + >> UOTGHS_DEVDMASTATUS_BUFF_COUNT_Pos; + if (nb_remaining) { + // Transfer no complete (short packet or ZLP) then: + // Update number of data transferred + ptr_job->buf_cnt -= nb_remaining; + // Set transfer complete to stop the transfer + ptr_job->buf_size = ptr_job->buf_cnt; + } + udd_ep_trans_done(ep); + return true; + } +#endif +#ifdef UDD_EP_FIFO_SUPPORTED + // Check RXRDY and TXEMPTY event for none DMA endpoints + if (!Is_udd_endpoint_dma_supported(ep) + && Is_udd_endpoint_interrupt_enabled(ep)) { + dbg_print("ep%x: ", ep); + // RXOUT: Full packet received + if (Is_udd_out_received(ep) + && Is_udd_out_received_interrupt_enabled(ep)) { + dbg_print("Out "); + udd_ep_out_received(ep); + return true; + } + // TXIN: packet sent + if (Is_udd_in_send(ep) + && Is_udd_in_send_interrupt_enabled(ep)) { + dbg_print("In "); + udd_ep_in_sent(ep); + return true; + } + // Errors: Abort? + if (Is_udd_overflow(ep) + || Is_udd_underflow(ep) + || Is_udd_crc_error(ep)) { + dbg_print("Err "); + udd_ep_abort(ep); + return true; + } + } +#endif // UDD_EP_FIFO_SUPPORTED + // Check empty bank interrupt event + if (Is_udd_endpoint_interrupt_enabled(ep)) { + dbg_print("bg%x: ", ep); + if (Is_udd_in_send_interrupt_enabled(ep) + && Is_udd_in_send(ep)) { + dbg_print("I "); + udd_disable_in_send_interrupt(ep); + // One bank is free then send a ZLP + udd_ack_in_send(ep); + udd_ack_fifocon(ep); + udd_ep_finish_job(ptr_job, false, ep); + return true; + } + if (Is_udd_bank_interrupt_enabled(ep) + && (0 == udd_nb_busy_bank(ep))) { + dbg_print("EoT "); + // End of background transfer on IN endpoint + udd_disable_bank_interrupt(ep); + udd_disable_endpoint_interrupt(ep); + + Assert(ptr_job->stall_requested); + // A stall has been requested during background transfer + ptr_job->stall_requested = false; + udd_disable_endpoint_bank_autoswitch(ep); + udd_enable_stall_handshake(ep); + udd_reset_data_toggle(ep); + return true; + } + } + } + return false; +} +#endif // (0 != USB_DEVICE_MAX_EP) + +//@} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/uotghs_device_due.h b/src/HAL/DUE/usb/uotghs_device_due.h new file mode 100644 index 0000000..6df26d6 --- /dev/null +++ b/src/HAL/DUE/usb/uotghs_device_due.h @@ -0,0 +1,664 @@ +/** + * \file + * + * \brief USB Device Driver for UOTGHS. Compliant with common UDD driver. + * + * Copyright (c) 2014-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef UOTGHS_DEVICE_DUE_H_INCLUDED +#define UOTGHS_DEVICE_DUE_H_INCLUDED + +//#include "compiler.h" + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +extern "C" { +#endif +/**INDENT-ON**/ +/// @endcond + +//! \ingroup udd_group +//! \defgroup udd_udphs_group USB On-The-Go High-Speed Port for device mode (UOTGHS) +//! UOTGHS low-level driver for USB device mode +//! +//! @{ + +#ifndef UOTGHS_DEVEPTCFG_EPDIR_Pos +// Bit pos is not defined in SAM header file but we need it. +# define UOTGHS_DEVEPTCFG_EPDIR_Pos 8 +#endif + +//! @name UOTGHS Device IP properties +//! These macros give access to IP properties +//! @{ + //! Get maximal number of endpoints +#define udd_get_endpoint_max_nbr() (9) +#define UDD_MAX_PEP_NB (udd_get_endpoint_max_nbr() + 1) + //! Get maximal number of banks of endpoints +#define udd_get_endpoint_bank_max_nbr(ep) ((ep == 0) ? 1 : (( ep <= 2) ? 3 : 2)) + //! Get maximal size of endpoint (3X, 1024/64) +#define udd_get_endpoint_size_max(ep) (((ep) == 0) ? 64 : 1024) + //! Get DMA support of endpoints +#define Is_udd_endpoint_dma_supported(ep) ((((ep) >= 1) && ((ep) <= 6)) ? true : false) + //! Get High Band Width support of endpoints +#define Is_udd_endpoint_high_bw_supported(ep) (((ep) >= 2) ? true : false) +//! @} + +//! @name UOTGHS Device speeds management +//! @{ + //! Enable/disable device low-speed mode +#define udd_low_speed_enable() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_LS)) +#define udd_low_speed_disable() (Clr_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_LS)) + //! Test if device low-speed mode is forced +#define Is_udd_low_speed_enable() (Tst_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_LS)) + +#ifdef UOTGHS_DEVCTRL_SPDCONF_HIGH_SPEED + //! Enable high speed mode +# define udd_high_speed_enable() (Wr_bitfield(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_SPDCONF_Msk, 0)) + //! Disable high speed mode +# define udd_high_speed_disable() (Wr_bitfield(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_SPDCONF_Msk, 3)) + //! Test if controller is in full speed mode +# define Is_udd_full_speed_mode() (Rd_bitfield(UOTGHS->UOTGHS_SR, UOTGHS_SR_SPEED_Msk) == UOTGHS_SR_SPEED_FULL_SPEED) +#else +# define udd_high_speed_enable() do { } while (0) +# define udd_high_speed_disable() do { } while (0) +# define Is_udd_full_speed_mode() true +#endif +//! @} + +//! @name UOTGHS Device HS test mode management +//! @{ +#ifdef UOTGHS_DEVCTRL_SPDCONF_HIGH_SPEED + //! Enable high speed test mode +# define udd_enable_hs_test_mode() (Wr_bitfield(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_SPDCONF_Msk, 2)) +# define udd_enable_hs_test_mode_j() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_TSTJ)) +# define udd_enable_hs_test_mode_k() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_TSTK)) +# define udd_enable_hs_test_mode_packet() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_TSTPCKT)) +#endif +//! @} + +//! @name UOTGHS Device vbus management +//! @{ +#define udd_enable_vbus_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define udd_disable_vbus_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define Is_udd_vbus_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define Is_udd_vbus_high() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBUS)) +#define Is_udd_vbus_low() (!Is_udd_vbus_high()) +#define udd_ack_vbus_transition() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_VBUSTIC) +#define udd_raise_vbus_transition() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_VBUSTIS) +#define Is_udd_vbus_transition() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBUSTI)) +//! @} + + +//! @name UOTGHS device attach control +//! These macros manage the UOTGHS Device attach. +//! @{ + //! Detaches from USB bus +#define udd_detach_device() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_DETACH)) + //! Attaches to USB bus +#define udd_attach_device() (Clr_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_DETACH)) + //! Test if the device is detached +#define Is_udd_detached() (Tst_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_DETACH)) +//! @} + + +//! @name UOTGHS device bus events control +//! These macros manage the UOTGHS Device bus events. +//! @{ + +//! Initiates a remote wake-up event +//! @{ +#define udd_initiate_remote_wake_up() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_RMWKUP)) +#define Is_udd_pending_remote_wake_up() (Tst_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_RMWKUP)) +//! @} + +//! Manage upstream resume event (=remote wakeup) +//! The USB driver sends a resume signal called "Upstream Resume" +//! @{ +#define udd_enable_remote_wake_up_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_UPRSMES) +#define udd_disable_remote_wake_up_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_UPRSMEC) +#define Is_udd_remote_wake_up_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_UPRSME)) +#define udd_ack_remote_wake_up_start() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_UPRSMC) +#define udd_raise_remote_wake_up_start() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_UPRSMS) +#define Is_udd_remote_wake_up_start() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_UPRSM)) +//! @} + +//! Manage downstream resume event (=remote wakeup from host) +//! The USB controller detects a valid "End of Resume" signal initiated by the host +//! @{ +#define udd_enable_resume_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_EORSMES) +#define udd_disable_resume_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_EORSMEC) +#define Is_udd_resume_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_EORSME)) +#define udd_ack_resume() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_EORSMC) +#define udd_raise_resume() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_EORSMS) +#define Is_udd_resume() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_EORSM)) +//! @} + +//! Manage wake-up event (=usb line activity) +//! The USB controller is reactivated by a filtered non-idle signal from the lines +//! @{ +#define udd_enable_wake_up_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_WAKEUPES) +#define udd_disable_wake_up_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_WAKEUPEC) +#define Is_udd_wake_up_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_WAKEUPE)) +#define udd_ack_wake_up() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_WAKEUPC) +#define udd_raise_wake_up() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_WAKEUPS) +#define Is_udd_wake_up() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_WAKEUP)) +//! @} + +//! Manage reset event +//! Set when a USB "End of Reset" has been detected +//! @{ +#define udd_enable_reset_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_EORSTES) +#define udd_disable_reset_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_EORSTEC) +#define Is_udd_reset_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_EORSTE)) +#define udd_ack_reset() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_EORSTC) +#define udd_raise_reset() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_EORSTS) +#define Is_udd_reset() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_EORST)) +//! @} + +//! Manage start of frame event +//! @{ +#define udd_enable_sof_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_SOFES) +#define udd_disable_sof_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_SOFEC) +#define Is_udd_sof_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_SOFE)) +#define udd_ack_sof() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_SOFC) +#define udd_raise_sof() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_SOFS) +#define Is_udd_sof() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_SOF)) +#define udd_frame_number() (Rd_bitfield(UOTGHS->UOTGHS_DEVFNUM, UOTGHS_DEVFNUM_FNUM_Msk)) +#define Is_udd_frame_number_crc_error() (Tst_bits(UOTGHS->UOTGHS_DEVFNUM, UOTGHS_DEVFNUM_FNCERR)) +//! @} + +//! Manage Micro start of frame event (High Speed Only) +//! @{ +#define udd_enable_msof_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_MSOFES) +#define udd_disable_msof_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_MSOFEC) +#define Is_udd_msof_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_MSOFE)) +#define udd_ack_msof() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVIMR_MSOFE) +#define udd_raise_msof() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_MSOFS) +#define Is_udd_msof() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_MSOF)) +#define udd_micro_frame_number() \ + (Rd_bitfield(UOTGHS->UOTGHS_DEVFNUM, (UOTGHS_DEVFNUM_FNUM_Msk|UOTGHS_DEVFNUM_MFNUM_Msk))) +//! @} + +//! Manage suspend event +//! @{ +#define udd_enable_suspend_interrupt() (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_SUSPES) +#define udd_disable_suspend_interrupt() (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_SUSPEC) +#define Is_udd_suspend_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_SUSPE)) +#define udd_ack_suspend() (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVICR_SUSPC) +#define udd_raise_suspend() (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_SUSPS) +#define Is_udd_suspend() (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_SUSP)) +//! @} + +//! @} + +//! @name UOTGHS device address control +//! These macros manage the UOTGHS Device address. +//! @{ + //! enables USB device address +#define udd_enable_address() (Set_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_ADDEN)) + //! disables USB device address +#define udd_disable_address() (Clr_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_ADDEN)) +#define Is_udd_address_enabled() (Tst_bits(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_ADDEN)) + //! configures the USB device address +#define udd_configure_address(addr) (Wr_bitfield(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_UADD_Msk, addr)) + //! gets the currently configured USB device address +#define udd_get_configured_address() (Rd_bitfield(UOTGHS->UOTGHS_DEVCTRL, UOTGHS_DEVCTRL_UADD_Msk)) +//! @} + + +//! @name UOTGHS Device endpoint drivers +//! These macros manage the common features of the endpoints. +//! @{ + +//! Generic macro for UOTGHS registers that can be arrayed +//! @{ +#define UOTGHS_ARRAY(reg,index) ((&(UOTGHS->reg))[(index)]) +//! @} + +//! @name UOTGHS Device endpoint configuration +//! @{ + //! enables the selected endpoint +#define udd_enable_endpoint(ep) (Set_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPEN0 << (ep))) + //! disables the selected endpoint +#define udd_disable_endpoint(ep) (Clr_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPEN0 << (ep))) + //! tests if the selected endpoint is enabled +#define Is_udd_endpoint_enabled(ep) (Tst_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPEN0 << (ep))) + //! resets the selected endpoint +#define udd_reset_endpoint(ep) \ + do { \ + Set_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPRST0 << (ep)); \ + Clr_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPRST0 << (ep)); \ + } while (0) + //! Tests if the selected endpoint is being reset +#define Is_udd_resetting_endpoint(ep) (Tst_bits(UOTGHS->UOTGHS_DEVEPT, UOTGHS_DEVEPT_EPRST0 << (ep))) + + //! Configures the selected endpoint type +#define udd_configure_endpoint_type(ep, type) (Wr_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPTYPE_Msk, type)) + //! Gets the configured selected endpoint type +#define udd_get_endpoint_type(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPTYPE_Msk)) + //! Enables the bank autoswitch for the selected endpoint +#define udd_enable_endpoint_bank_autoswitch(ep) (Set_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_AUTOSW)) + //! Disables the bank autoswitch for the selected endpoint +#define udd_disable_endpoint_bank_autoswitch(ep) (Clr_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_AUTOSW)) +#define Is_udd_endpoint_bank_autoswitch_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_AUTOSW)) + //! Configures the selected endpoint direction +#define udd_configure_endpoint_direction(ep, dir) (Wr_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPDIR, dir)) + //! Gets the configured selected endpoint direction +#define udd_get_endpoint_direction(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPDIR)) +#define Is_udd_endpoint_in(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPDIR)) + //! Bounds given integer size to allowed range and rounds it up to the nearest + //! available greater size, then applies register format of UOTGHS controller + //! for endpoint size bit-field. +#undef udd_format_endpoint_size +#define udd_format_endpoint_size(size) (32 - clz(((uint32_t)min(max(size, 8), 1024) << 1) - 1) - 1 - 3) + //! Configures the selected endpoint size +#define udd_configure_endpoint_size(ep, size) (Wr_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPSIZE_Msk, udd_format_endpoint_size(size))) + //! Gets the configured selected endpoint size +#define udd_get_endpoint_size(ep) (8 << Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPSIZE_Msk)) + //! Configures the selected endpoint number of banks +#define udd_configure_endpoint_bank(ep, bank) (Wr_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPBK_Msk, bank)) + //! Gets the configured selected endpoint number of banks +#define udd_get_endpoint_bank(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPBK_Msk)+1) + //! Allocates the configuration selected endpoint in DPRAM memory +#define udd_allocate_memory(ep) (Set_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_ALLOC)) + //! un-allocates the configuration selected endpoint in DPRAM memory +#define udd_unallocate_memory(ep) (Clr_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_ALLOC)) +#define Is_udd_memory_allocated(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_ALLOC)) + + //! Configures selected endpoint in one step +#define udd_configure_endpoint(ep, type, dir, size, bank) (\ + Wr_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTCFG[0], ep), UOTGHS_DEVEPTCFG_EPTYPE_Msk |\ + UOTGHS_DEVEPTCFG_EPDIR |\ + UOTGHS_DEVEPTCFG_EPSIZE_Msk |\ + UOTGHS_DEVEPTCFG_EPBK_Msk , \ + (((uint32_t)(type) << UOTGHS_DEVEPTCFG_EPTYPE_Pos) & UOTGHS_DEVEPTCFG_EPTYPE_Msk) |\ + (((uint32_t)(dir ) << UOTGHS_DEVEPTCFG_EPDIR_Pos ) & UOTGHS_DEVEPTCFG_EPDIR) |\ + ( (uint32_t)udd_format_endpoint_size(size) << UOTGHS_DEVEPTCFG_EPSIZE_Pos) |\ + (((uint32_t)(bank) << UOTGHS_DEVEPTCFG_EPBK_Pos) & UOTGHS_DEVEPTCFG_EPBK_Msk))\ +) + //! Tests if current endpoint is configured +#define Is_udd_endpoint_configured(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_CFGOK)) + //! Returns the control direction +#define udd_control_direction() (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], EP_CONTROL), UOTGHS_DEVEPTISR_CTRLDIR)) + + //! Resets the data toggle sequence +#define udd_reset_data_toggle(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_RSTDTS) + //! Tests if the data toggle sequence is being reset +#define Is_udd_data_toggle_reset(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_RSTDT)) + //! Returns data toggle +#define udd_data_toggle(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_DTSEQ_Msk)) +//! @} + + +//! @name UOTGHS Device control endpoint +//! These macros control the endpoints. +//! @{ + +//! @name UOTGHS Device control endpoint interrupts +//! These macros control the endpoints interrupts. +//! @{ + //! Enables the selected endpoint interrupt +#define udd_enable_endpoint_interrupt(ep) (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_PEP_0 << (ep)) + //! Disables the selected endpoint interrupt +#define udd_disable_endpoint_interrupt(ep) (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_PEP_0 << (ep)) + //! Tests if the selected endpoint interrupt is enabled +#define Is_udd_endpoint_interrupt_enabled(ep) (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_PEP_0 << (ep))) + //! Tests if an interrupt is triggered by the selected endpoint +#define Is_udd_endpoint_interrupt(ep) (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_PEP_0 << (ep))) + //! Returns the lowest endpoint number generating an endpoint interrupt or MAX_PEP_NB if none +#define udd_get_interrupt_endpoint_number() (ctz(((UOTGHS->UOTGHS_DEVISR >> UOTGHS_DEVISR_PEP_Pos) & \ + (UOTGHS->UOTGHS_DEVIMR >> UOTGHS_DEVIMR_PEP_Pos)) | \ + (1 << MAX_PEP_NB))) +#define UOTGHS_DEVISR_PEP_Pos 12 +#define UOTGHS_DEVIMR_PEP_Pos 12 +//! @} + +//! @name UOTGHS Device control endpoint errors +//! These macros control the endpoint errors. +//! @{ + //! Enables the STALL handshake +#define udd_enable_stall_handshake(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_STALLRQS) + //! Disables the STALL handshake +#define udd_disable_stall_handshake(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_STALLRQC) + //! Tests if STALL handshake request is running +#define Is_udd_endpoint_stall_requested(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_STALLRQ)) + //! Tests if STALL sent +#define Is_udd_stall(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_STALLEDI)) + //! ACKs STALL sent +#define udd_ack_stall(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_STALLEDIC) + //! Raises STALL sent +#define udd_raise_stall(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_STALLEDIS) + //! Enables STALL sent interrupt +#define udd_enable_stall_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_STALLEDES) + //! Disables STALL sent interrupt +#define udd_disable_stall_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_STALLEDEC) + //! Tests if STALL sent interrupt is enabled +#define Is_udd_stall_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_STALLEDE)) + + //! Tests if NAK OUT received +#define Is_udd_nak_out(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_NAKOUTI)) + //! ACKs NAK OUT received +#define udd_ack_nak_out(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_NAKOUTIC) + //! Raises NAK OUT received +#define udd_raise_nak_out(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_NAKOUTIS) + //! Enables NAK OUT interrupt +#define udd_enable_nak_out_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_NAKOUTES) + //! Disables NAK OUT interrupt +#define udd_disable_nak_out_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_NAKOUTEC) + //! Tests if NAK OUT interrupt is enabled +#define Is_udd_nak_out_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_NAKOUTE)) + + //! Tests if NAK IN received +#define Is_udd_nak_in(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_NAKINI)) + //! ACKs NAK IN received +#define udd_ack_nak_in(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_NAKINIC) + //! Raises NAK IN received +#define udd_raise_nak_in(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_NAKINIS) + //! Enables NAK IN interrupt +#define udd_enable_nak_in_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_NAKINES) + //! Disables NAK IN interrupt +#define udd_disable_nak_in_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_NAKINEC) + //! Tests if NAK IN interrupt is enabled +#define Is_udd_nak_in_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_NAKINE)) + + //! ACKs endpoint isochronous overflow interrupt +#define udd_ack_overflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_OVERFIC) + //! Raises endpoint isochronous overflow interrupt +#define udd_raise_overflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_OVERFIS) + //! Tests if an overflow occurs +#define Is_udd_overflow(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_OVERFI)) + //! Enables overflow interrupt +#define udd_enable_overflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_OVERFES) + //! Disables overflow interrupt +#define udd_disable_overflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_OVERFEC) + //! Tests if overflow interrupt is enabled +#define Is_udd_overflow_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_OVERFE)) + + //! ACKs endpoint isochronous underflow interrupt +#define udd_ack_underflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_UNDERFIC) + //! Raises endpoint isochronous underflow interrupt +#define udd_raise_underflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_UNDERFIS) + //! Tests if an underflow occurs +#define Is_udd_underflow(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_UNDERFI)) + //! Enables underflow interrupt +#define udd_enable_underflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_UNDERFES) + //! Disables underflow interrupt +#define udd_disable_underflow_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_UNDERFEC) + //! Tests if underflow interrupt is enabled +#define Is_udd_underflow_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_UNDERFE)) + + //! Tests if CRC ERROR ISO OUT detected +#define Is_udd_crc_error(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_CRCERRI)) + //! ACKs CRC ERROR ISO OUT detected +#define udd_ack_crc_error(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_CRCERRIC) + //! Raises CRC ERROR ISO OUT detected +#define udd_raise_crc_error(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_CRCERRIS) + //! Enables CRC ERROR ISO OUT detected interrupt +#define udd_enable_crc_error_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_CRCERRES) + //! Disables CRC ERROR ISO OUT detected interrupt +#define udd_disable_crc_error_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_CRCERREC) + //! Tests if CRC ERROR ISO OUT detected interrupt is enabled +#define Is_udd_crc_error_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_CRCERRE)) +//! @} + +//! @name UOTGHS Device control endpoint transfer +//! These macros control the endpoint transfer. +//! @{ + + //! Tests if endpoint read allowed +#define Is_udd_read_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_RWALL)) + //! Tests if endpoint write allowed +#define Is_udd_write_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_RWALL)) + + //! Returns the byte count +#define udd_byte_count(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_BYCT_Msk)) + //! Clears FIFOCON bit +#define udd_ack_fifocon(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_FIFOCONC) + //! Tests if FIFOCON bit set +#define Is_udd_fifocon(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_FIFOCON)) + + //! Returns the number of busy banks +#define udd_nb_busy_bank(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_NBUSYBK_Msk)) + //! Returns the number of the current bank +#define udd_current_bank(ep) (Rd_bitfield(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_CURRBK_Msk)) + //! Kills last bank +#define udd_kill_last_in_bank(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_KILLBKS) +#define Is_udd_kill_last(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_KILLBK)) + //! Tests if last bank killed +#define Is_udd_last_in_bank_killed(ep) (!Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_KILLBK)) + //! Forces all banks full (OUT) or free (IN) interrupt +#define udd_force_bank_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_NBUSYBKS) + //! Unforces all banks full (OUT) or free (IN) interrupt +#define udd_unforce_bank_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_NBUSYBKS) + //! Enables all banks full (OUT) or free (IN) interrupt +#define udd_enable_bank_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_NBUSYBKES) + //! Disables all banks full (OUT) or free (IN) interrupt +#define udd_disable_bank_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_NBUSYBKEC) + //! Tests if all banks full (OUT) or free (IN) interrupt enabled +#define Is_udd_bank_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_NBUSYBKE)) + + //! Tests if SHORT PACKET received +#define Is_udd_short_packet(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_SHORTPACKET)) + //! ACKs SHORT PACKET received +#define udd_ack_short_packet(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_SHORTPACKETC) + //! Raises SHORT PACKET received +#define udd_raise_short_packet(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_SHORTPACKETS) + //! Enables SHORT PACKET received interrupt +#define udd_enable_short_packet_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_SHORTPACKETES) + //! Disables SHORT PACKET received interrupt +#define udd_disable_short_packet_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_SHORTPACKETEC) + //! Tests if SHORT PACKET received interrupt is enabled +#define Is_udd_short_packet_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_SHORTPACKETE)) + + //! Tests if SETUP received +#define Is_udd_setup_received(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_RXSTPI)) + //! ACKs SETUP received +#define udd_ack_setup_received(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_RXSTPIC) + //! Raises SETUP received +#define udd_raise_setup_received(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_RXSTPIS) + //! Enables SETUP received interrupt +#define udd_enable_setup_received_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_RXSTPES) + //! Disables SETUP received interrupt +#define udd_disable_setup_received_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_RXSTPEC) + //! Tests if SETUP received interrupt is enabled +#define Is_udd_setup_received_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_RXSTPE)) + + //! Tests if OUT received +#define Is_udd_out_received(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_RXOUTI)) + //! ACKs OUT received +#define udd_ack_out_received(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_RXOUTIC) + //! Raises OUT received +#define udd_raise_out_received(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_RXOUTIS) + //! Enables OUT received interrupt +#define udd_enable_out_received_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_RXOUTES) + //! Disables OUT received interrupt +#define udd_disable_out_received_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_RXOUTEC) + //! Tests if OUT received interrupt is enabled +#define Is_udd_out_received_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_RXOUTE)) + + //! Tests if IN sending +#define Is_udd_in_send(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTISR[0], ep), UOTGHS_DEVEPTISR_TXINI)) + //! ACKs IN sending +#define udd_ack_in_send(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTICR[0], ep) = UOTGHS_DEVEPTICR_TXINIC) + //! Raises IN sending +#define udd_raise_in_send(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIFR[0], ep) = UOTGHS_DEVEPTIFR_TXINIS) + //! Enables IN sending interrupt +#define udd_enable_in_send_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0], ep) = UOTGHS_DEVEPTIER_TXINES) + //! Disables IN sending interrupt +#define udd_disable_in_send_interrupt(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0], ep) = UOTGHS_DEVEPTIDR_TXINEC) + //! Tests if IN sending interrupt is enabled +#define Is_udd_in_send_interrupt_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0], ep), UOTGHS_DEVEPTIMR_TXINE)) + + + //! Get 64-, 32-, 16- or 8-bit access to FIFO data register of selected endpoint. + //! @param ep Endpoint of which to access FIFO data register + //! @param scale Data scale in bits: 64, 32, 16 or 8 + //! @return Volatile 64-, 32-, 16- or 8-bit data pointer to FIFO data register + //! @warning It is up to the user of this macro to make sure that all accesses + //! are aligned with their natural boundaries except 64-bit accesses which + //! require only 32-bit alignment. + //! @warning It is up to the user of this macro to make sure that used HSB + //! addresses are identical to the DPRAM internal pointer modulo 32 bits. +#define udd_get_endpoint_fifo_access(ep, scale) \ + (((volatile TPASTE2(U, scale) (*)[0x8000 / ((scale) / 8)])UOTGHS_RAM_ADDR)[(ep)]) + +//! @name UOTGHS endpoint DMA drivers +//! These macros manage the common features of the endpoint DMA channels. +//! @{ + + //! Maximum transfer size on USB DMA +#define UDD_ENDPOINT_MAX_TRANS 0x10000 + //! Enables the disabling of HDMA requests by endpoint interrupts +#define udd_enable_endpoint_int_dis_hdma_req(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIER[0](ep) = UOTGHS_DEVEPTIER_EPDISHDMAS) + //! Disables the disabling of HDMA requests by endpoint interrupts +#define udd_disable_endpoint_int_dis_hdma_req(ep) (UOTGHS_ARRAY(UOTGHS_DEVEPTIDR[0](ep) = UOTGHS_DEVEPTIDR_EPDISHDMAC) + //! Tests if the disabling of HDMA requests by endpoint interrupts is enabled +#define Is_udd_endpoint_int_dis_hdma_req_enabled(ep) (Tst_bits(UOTGHS_ARRAY(UOTGHS_DEVEPTIMR[0](ep), UOTGHS_DEVEPTIMR_EPDISHDMA)) + + //! Raises the selected endpoint DMA channel interrupt +#define udd_raise_endpoint_dma_interrupt(ep) (UOTGHS->UOTGHS_DEVIFR = UOTGHS_DEVIFR_DMA_1 << ((ep) - 1)) + //! Raises the selected endpoint DMA channel interrupt +#define udd_clear_endpoint_dma_interrupt(ep) (UOTGHS->UOTGHS_DEVICR = UOTGHS_DEVISR_DMA_1 << ((ep) - 1)) + //! Tests if an interrupt is triggered by the selected endpoint DMA channel +#define Is_udd_endpoint_dma_interrupt(ep) (Tst_bits(UOTGHS->UOTGHS_DEVISR, UOTGHS_DEVISR_DMA_1 << ((ep) - 1))) + //! Enables the selected endpoint DMA channel interrupt +#define udd_enable_endpoint_dma_interrupt(ep) (UOTGHS->UOTGHS_DEVIER = UOTGHS_DEVIER_DMA_1 << ((ep) - 1)) + //! Disables the selected endpoint DMA channel interrupt +#define udd_disable_endpoint_dma_interrupt(ep) (UOTGHS->UOTGHS_DEVIDR = UOTGHS_DEVIDR_DMA_1 << ((ep) - 1)) + //! Tests if the selected endpoint DMA channel interrupt is enabled +#define Is_udd_endpoint_dma_interrupt_enabled(ep) (Tst_bits(UOTGHS->UOTGHS_DEVIMR, UOTGHS_DEVIMR_DMA_1 << ((ep) - 1))) + + //! Access points to the UOTGHS device DMA memory map with arrayed registers + //! @{ + //! Structure for DMA next descriptor register +typedef struct { + uint32_t *NXT_DSC_ADD; +} uotghs_dma_nextdesc_t; + //! Structure for DMA control register +typedef struct { + uint32_t CHANN_ENB:1, + LDNXT_DSC:1, + END_TR_EN:1, + END_B_EN:1, + END_TR_IT:1, + END_BUFFIT:1, + DESC_LD_IT:1, + BUST_LCK:1, + reserved:8, + BUFF_LENGTH:16; +} uotghs_dma_control_t; + //! Structure for DMA status register +typedef struct { + uint32_t CHANN_ENB:1, + CHANN_ACT:1, + reserved0:2, + END_TR_ST:1, + END_BF_ST:1, + DESC_LDST:1, + reserved1:9, + BUFF_COUNT:16; +} uotghs_dma_status_t; + //! Structure for DMA descriptor +typedef struct { + union { + uint32_t nextdesc; + uotghs_dma_nextdesc_t NEXTDESC; + }; + uint32_t addr; + union { + uint32_t control; + uotghs_dma_control_t CONTROL; + }; + uint32_t reserved; +} sam_uotghs_dmadesc_t, uotghs_dmadesc_t; + //! Structure for DMA registers in a channel +typedef struct { + union { + uint32_t nextdesc; + uotghs_dma_nextdesc_t NEXTDESC; + }; + uint32_t addr; + union { + uint32_t control; + uotghs_dma_control_t CONTROL; + }; + union { + unsigned long status; + uotghs_dma_status_t STATUS; + }; +} sam_uotghs_dmach_t, uotghs_dmach_t; + //! DMA channel control command +#define UDD_ENDPOINT_DMA_STOP_NOW (0) +#define UDD_ENDPOINT_DMA_RUN_AND_STOP (UOTGHS_DEVDMACONTROL_CHANN_ENB) +#define UDD_ENDPOINT_DMA_LOAD_NEXT_DESC (UOTGHS_DEVDMACONTROL_LDNXT_DSC) +#define UDD_ENDPOINT_DMA_RUN_AND_LINK (UOTGHS_DEVDMACONTROL_CHANN_ENB|UOTGHS_DEVDMACONTROL_LDNXT_DSC) + //! Structure for DMA registers +#define UOTGHS_UDDMA_ARRAY(ep) (((volatile uotghs_dmach_t *)UOTGHS->UOTGHS_DEVDMA)[(ep) - 1]) + + //! Set control desc to selected endpoint DMA channel +#define udd_endpoint_dma_set_control(ep,desc) (UOTGHS_UDDMA_ARRAY(ep).control = desc) + //! Get control desc to selected endpoint DMA channel +#define udd_endpoint_dma_get_control(ep) (UOTGHS_UDDMA_ARRAY(ep).control) + //! Set RAM address to selected endpoint DMA channel +#define udd_endpoint_dma_set_addr(ep,add) (UOTGHS_UDDMA_ARRAY(ep).addr = add) + //! Get status to selected endpoint DMA channel +#define udd_endpoint_dma_get_status(ep) (UOTGHS_UDDMA_ARRAY(ep).status) + //! @} +//! @} + +//! @} +//! @} +//! @} +//! @} + + +/// @cond 0 +/**INDENT-OFF**/ +#ifdef __cplusplus +} +#endif +/**INDENT-ON**/ +/// @endcond + +#endif /* UOTGHS_DEVICE_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/uotghs_otg.h b/src/HAL/DUE/usb/uotghs_otg.h new file mode 100644 index 0000000..eca5e93 --- /dev/null +++ b/src/HAL/DUE/usb/uotghs_otg.h @@ -0,0 +1,241 @@ +/** + * \file + * + * \brief USB OTG Driver for UOTGHS. + * + * Copyright (c) 2012-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef UOTGHS_OTG_H_INCLUDED +#define UOTGHS_OTG_H_INCLUDED + +#include "compiler.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +//! \ingroup usb_group +//! \defgroup otg_group UOTGHS OTG Driver +//! UOTGHS low-level driver for OTG features +//! +//! @{ + +/** + * \brief Initialize the dual role + * This function is implemented in uotghs_host.c file. + * + * \return \c true if the ID pin management has been started, otherwise \c false. + */ +bool otg_dual_enable(void); + +/** + * \brief Uninitialize the dual role + * This function is implemented in uotghs_host.c file. + */ +void otg_dual_disable(void); + + +//! @name UOTGHS OTG ID pin management +//! The ID pin come from the USB OTG connector (A and B receptable) and +//! allows to select the USB mode host or device. +//! The UOTGHS hardware can manage it automatically. This feature is optional. +//! When USB_ID_GPIO is defined (in board.h), this feature is enabled. +//! +//! @{ + //! Enable external OTG_ID pin (listened to by USB) +#define otg_enable_id_pin() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIDE)) + //! Disable external OTG_ID pin (ignored by USB) +#define otg_disable_id_pin() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIDE)) + //! Test if external OTG_ID pin enabled (listened to by USB) +#define Is_otg_id_pin_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIDE)) + //! Disable external OTG_ID pin and force device mode +#define otg_force_device_mode() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIMOD), otg_disable_id_pin()) + //! Test if device mode is forced +#define Is_otg_device_mode_forced() (!Is_otg_id_pin_enabled() && Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIMOD)) + //! Disable external OTG_ID pin and force host mode +#define otg_force_host_mode() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIMOD), otg_disable_id_pin()) + //! Test if host mode is forced +#define Is_otg_host_mode_forced() (!Is_otg_id_pin_enabled() && !Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UIMOD)) + +//! @name UOTGHS OTG ID pin interrupt management +//! These macros manage the ID pin interrupt +//! @{ +#define otg_enable_id_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_IDTE)) +#define otg_disable_id_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_IDTE)) +#define Is_otg_id_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_IDTE)) +#define Is_otg_id_device() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_ID)) +#define Is_otg_id_host() (!Is_otg_id_device()) +#define otg_ack_id_transition() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_IDTIC) +#define otg_raise_id_transition() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_IDTIS) +#define Is_otg_id_transition() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_IDTI)) +//! @} +//! @} + +//! @name OTG Vbus management +//! @{ +#define otg_enable_vbus_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define otg_disable_vbus_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define Is_otg_vbus_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_VBUSTE)) +#define Is_otg_vbus_high() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBUS)) +#define Is_otg_vbus_low() (!Is_otg_vbus_high()) +#define otg_ack_vbus_transition() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_VBUSTIC) +#define otg_raise_vbus_transition() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_VBUSTIS) +#define Is_otg_vbus_transition() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_VBUSTI)) +//! @} + +//! @name UOTGHS OTG main management +//! These macros allows to enable/disable pad and UOTGHS hardware +//! @{ + //! Reset USB macro +#define otg_reset() \ + do { \ + UOTGHS->UOTGHS_CTRL = 0; \ + while( UOTGHS->UOTGHS_SR & 0x3FFF) {\ + UOTGHS->UOTGHS_SCR = 0xFFFFFFFF;\ + } \ + } while (0) + //! Enable USB macro +#define otg_enable() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_USBE)) + //! Disable USB macro +#define otg_disable() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_USBE)) +#define Is_otg_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_USBE)) + + //! Enable OTG pad +#define otg_enable_pad() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_OTGPADE)) + //! Disable OTG pad +#define otg_disable_pad() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_OTGPADE)) +#define Is_otg_pad_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_OTGPADE)) + + //! Check Clock Usable + //! For parts with HS feature, this one corresponding at UTMI clock +#define Is_otg_clock_usable() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_CLKUSABLE)) + + //! Stop (freeze) internal USB clock +#define otg_freeze_clock() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK)) +#define otg_unfreeze_clock() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK)) +#define Is_otg_clock_frozen() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_FRZCLK)) + + //! Configure time-out of specified OTG timer +#define otg_configure_timeout(timer, timeout) (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UNLOCK),\ + Wr_bitfield(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_TIMPAGE_Msk, timer),\ + Wr_bitfield(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_TIMVALUE_Msk, timeout),\ + Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UNLOCK)) + //! Get configured time-out of specified OTG timer +#define otg_get_timeout(timer) (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UNLOCK),\ + Wr_bitfield(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_TIMPAGE_Msk, timer),\ + Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_UNLOCK),\ + Rd_bitfield(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_TIMVALUE_Msk)) + + + //! Get the dual-role device state of the internal USB finite state machine of the UOTGHS controller +#define otg_get_fsm_drd_state() (Rd_bitfield(UOTGHS->UOTGHS_FSM, UOTGHS_FSM_DRDSTATE_Msk)) +#define Is_otg_a_suspend() (4==otg_get_fsm_drd_state()) +#define Is_otg_a_wait_vrise() (1==otg_get_fsm_drd_state()) +//! @} + +//! @name UOTGHS OTG hardware protocol +//! These macros manages the hardware OTG protocol +//! @{ + //! Initiates a Host negotiation Protocol +#define otg_device_initiate_hnp() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPREQ)) + //! Accepts a Host negotiation Protocol +#define otg_host_accept_hnp() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPREQ)) + //! Rejects a Host negotiation Protocol +#define otg_host_reject_hnp() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPREQ)) + //! initiates a Session Request Protocol +#define otg_device_initiate_srp() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPREQ)) + //! Selects VBus as SRP method +#define otg_select_vbus_srp_method() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPSEL)) +#define Is_otg_vbus_srp_method_selected() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPSEL)) + //! Selects data line as SRP method +#define otg_select_data_srp_method() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPSEL)) +#define Is_otg_data_srp_method_selected() (!Is_otg_vbus_srp_method_selected()) + //! Tests if a HNP occurs +#define Is_otg_hnp() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPREQ)) + //! Tests if a SRP from device occurs +#define Is_otg_device_srp() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPREQ)) + + //! Enables HNP error interrupt +#define otg_enable_hnp_error_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPERRE)) + //! Disables HNP error interrupt +#define otg_disable_hnp_error_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPERRE)) +#define Is_otg_hnp_error_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_HNPERRE)) + //! ACKs HNP error interrupt +#define otg_ack_hnp_error_interrupt() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_HNPERRIC) + //! Raises HNP error interrupt +#define otg_raise_hnp_error_interrupt() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_HNPERRIS) + //! Tests if a HNP error occurs +#define Is_otg_hnp_error_interrupt() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_HNPERRI)) + + //! Enables role exchange interrupt +#define otg_enable_role_exchange_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_ROLEEXE)) + //! Disables role exchange interrupt +#define otg_disable_role_exchange_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_ROLEEXE)) +#define Is_otg_role_exchange_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_ROLEEXE)) + //! ACKs role exchange interrupt +#define otg_ack_role_exchange_interrupt() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_ROLEEXIC) + //! Raises role exchange interrupt +#define otg_raise_role_exchange_interrupt() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_ROLEEXIS) + //! Tests if a role exchange occurs +#define Is_otg_role_exchange_interrupt() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_ROLEEXI)) + + //! Enables SRP interrupt +#define otg_enable_srp_interrupt() (Set_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPE)) + //! Disables SRP interrupt +#define otg_disable_srp_interrupt() (Clr_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPE)) +#define Is_otg_srp_interrupt_enabled() (Tst_bits(UOTGHS->UOTGHS_CTRL, UOTGHS_CTRL_SRPE)) + //! ACKs SRP interrupt +#define otg_ack_srp_interrupt() (UOTGHS->UOTGHS_SCR = UOTGHS_SCR_SRPIC) + //! Raises SRP interrupt +#define otg_raise_srp_interrupt() (UOTGHS->UOTGHS_SFR = UOTGHS_SFR_SRPIS) + //! Tests if a SRP occurs +#define Is_otg_srp_interrupt() (Tst_bits(UOTGHS->UOTGHS_SR, UOTGHS_SR_SRPI)) +//! @} + +//! @} + +#ifdef __cplusplus +} +#endif + +#endif /* UOTGHS_OTG_H_INCLUDED */ diff --git a/src/HAL/DUE/usb/usb_protocol.h b/src/HAL/DUE/usb/usb_protocol.h new file mode 100644 index 0000000..ea51a86 --- /dev/null +++ b/src/HAL/DUE/usb/usb_protocol.h @@ -0,0 +1,496 @@ +/** + * \file + * + * \brief USB protocol definitions. + * + * This file contains the USB definitions and data structures provided by the + * USB 2.0 specification. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _USB_PROTOCOL_H_ +#define _USB_PROTOCOL_H_ + +/** + * \ingroup usb_group + * \defgroup usb_protocol_group USB Protocol Definitions + * + * This module defines constants and data structures provided by the USB + * 2.0 specification. + * + * @{ + */ + +//! Value for field bcdUSB +#define USB_V2_0 0x0200 //!< USB Specification version 2.00 +#define USB_V2_1 0x0201 //!< USB Specification version 2.01 + +/*! \name Generic definitions (Class, subclass and protocol) + */ +//! @{ +#define NO_CLASS 0x00 +#define CLASS_VENDOR_SPECIFIC 0xFF +#define NO_SUBCLASS 0x00 +#define NO_PROTOCOL 0x00 +//! @} + +//! \name IAD (Interface Association Descriptor) constants +//! @{ +#define CLASS_IAD 0xEF +#define SUB_CLASS_IAD 0x02 +#define PROTOCOL_IAD 0x01 +//! @} + +/** + * \brief USB request data transfer direction (bmRequestType) + */ +#define USB_REQ_DIR_OUT (0<<7) //!< Host to device +#define USB_REQ_DIR_IN (1<<7) //!< Device to host +#define USB_REQ_DIR_MASK (1<<7) //!< Mask + +/** + * \brief USB request types (bmRequestType) + */ +#define USB_REQ_TYPE_STANDARD (0<<5) //!< Standard request +#define USB_REQ_TYPE_CLASS (1<<5) //!< Class-specific request +#define USB_REQ_TYPE_VENDOR (2<<5) //!< Vendor-specific request +#define USB_REQ_TYPE_MASK (3<<5) //!< Mask + +/** + * \brief USB recipient codes (bmRequestType) + */ +#define USB_REQ_RECIP_DEVICE (0<<0) //!< Recipient device +#define USB_REQ_RECIP_INTERFACE (1<<0) //!< Recipient interface +#define USB_REQ_RECIP_ENDPOINT (2<<0) //!< Recipient endpoint +#define USB_REQ_RECIP_OTHER (3<<0) //!< Recipient other +#define USB_REQ_RECIP_MASK (0x1F) //!< Mask + +/** + * \brief Standard USB requests (bRequest) + */ +enum usb_reqid { + USB_REQ_GET_STATUS = 0, + USB_REQ_CLEAR_FEATURE = 1, + USB_REQ_SET_FEATURE = 3, + USB_REQ_SET_ADDRESS = 5, + USB_REQ_GET_DESCRIPTOR = 6, + USB_REQ_SET_DESCRIPTOR = 7, + USB_REQ_GET_CONFIGURATION = 8, + USB_REQ_SET_CONFIGURATION = 9, + USB_REQ_GET_INTERFACE = 10, + USB_REQ_SET_INTERFACE = 11, + USB_REQ_SYNCH_FRAME = 12, +}; + +/** + * \brief Standard USB device status flags + * + */ +enum usb_device_status { + USB_DEV_STATUS_BUS_POWERED = 0, + USB_DEV_STATUS_SELF_POWERED = 1, + USB_DEV_STATUS_REMOTEWAKEUP = 2 +}; + +/** + * \brief Standard USB Interface status flags + * + */ +enum usb_interface_status { + USB_IFACE_STATUS_RESERVED = 0 +}; + +/** + * \brief Standard USB endpoint status flags + * + */ +enum usb_endpoint_status { + USB_EP_STATUS_HALTED = 1, +}; + +/** + * \brief Standard USB device feature flags + * + * \note valid for SetFeature request. + */ +enum usb_device_feature { + USB_DEV_FEATURE_REMOTE_WAKEUP = 1, //!< Remote wakeup enabled + USB_DEV_FEATURE_TEST_MODE = 2, //!< USB test mode + USB_DEV_FEATURE_OTG_B_HNP_ENABLE = 3, + USB_DEV_FEATURE_OTG_A_HNP_SUPPORT = 4, + USB_DEV_FEATURE_OTG_A_ALT_HNP_SUPPORT = 5 +}; + +/** + * \brief Test Mode possible on HS USB device + * + * \note valid for USB_DEV_FEATURE_TEST_MODE request. + */ +enum usb_device_hs_test_mode { + USB_DEV_TEST_MODE_J = 1, + USB_DEV_TEST_MODE_K = 2, + USB_DEV_TEST_MODE_SE0_NAK = 3, + USB_DEV_TEST_MODE_PACKET = 4, + USB_DEV_TEST_MODE_FORCE_ENABLE = 5, +}; + +/** + * \brief Standard USB endpoint feature/status flags + */ +enum usb_endpoint_feature { + USB_EP_FEATURE_HALT = 0, +}; + +/** + * \brief Standard USB Test Mode Selectors + */ +enum usb_test_mode_selector { + USB_TEST_J = 0x01, + USB_TEST_K = 0x02, + USB_TEST_SE0_NAK = 0x03, + USB_TEST_PACKET = 0x04, + USB_TEST_FORCE_ENABLE = 0x05, +}; + +/** + * \brief Standard USB descriptor types + */ +enum usb_descriptor_type { + USB_DT_DEVICE = 1, + USB_DT_CONFIGURATION = 2, + USB_DT_STRING = 3, + USB_DT_INTERFACE = 4, + USB_DT_ENDPOINT = 5, + USB_DT_DEVICE_QUALIFIER = 6, + USB_DT_OTHER_SPEED_CONFIGURATION = 7, + USB_DT_INTERFACE_POWER = 8, + USB_DT_OTG = 9, + USB_DT_IAD = 0x0B, + USB_DT_BOS = 0x0F, + USB_DT_DEVICE_CAPABILITY = 0x10, +}; + +/** + * \brief USB Device Capability types + */ +enum usb_capability_type { + USB_DC_USB20_EXTENSION = 0x02, +}; + +/** + * \brief USB Device Capability - USB 2.0 Extension + * To fill bmAttributes field of usb_capa_ext_desc_t structure. + */ +enum usb_capability_extension_attr { + USB_DC_EXT_LPM = 0x00000002, +}; + +#define HIRD_50_US 0 +#define HIRD_125_US 1 +#define HIRD_200_US 2 +#define HIRD_275_US 3 +#define HIRD_350_US 4 +#define HIRD_425_US 5 +#define HIRD_500_US 6 +#define HIRD_575_US 7 +#define HIRD_650_US 8 +#define HIRD_725_US 9 +#define HIRD_800_US 10 +#define HIRD_875_US 11 +#define HIRD_950_US 12 +#define HIRD_1025_US 13 +#define HIRD_1100_US 14 +#define HIRD_1175_US 15 + +/** Fields definition from a LPM TOKEN */ +#define USB_LPM_ATTRIBUT_BLINKSTATE_MASK (0xF << 0) +#define USB_LPM_ATTRIBUT_FIRD_MASK (0xF << 4) +#define USB_LPM_ATTRIBUT_REMOTEWAKE_MASK (1 << 8) +#define USB_LPM_ATTRIBUT_BLINKSTATE(value) ((value & 0xF) << 0) +#define USB_LPM_ATTRIBUT_FIRD(value) ((value & 0xF) << 4) +#define USB_LPM_ATTRIBUT_REMOTEWAKE(value) ((value & 1) << 8) +#define USB_LPM_ATTRIBUT_BLINKSTATE_L1 USB_LPM_ATTRIBUT_BLINKSTATE(1) + +/** + * \brief Standard USB endpoint transfer types + */ +enum usb_ep_type { + USB_EP_TYPE_CONTROL = 0x00, + USB_EP_TYPE_ISOCHRONOUS = 0x01, + USB_EP_TYPE_BULK = 0x02, + USB_EP_TYPE_INTERRUPT = 0x03, + USB_EP_TYPE_MASK = 0x03, +}; + +/** + * \brief Standard USB language IDs for string descriptors + */ +enum usb_langid { + USB_LANGID_EN_US = 0x0409, //!< English (United States) +}; + +/** + * \brief Mask selecting the index part of an endpoint address + */ +#define USB_EP_ADDR_MASK 0x0F + +//! \brief USB address identifier +typedef uint8_t usb_add_t; + +/** + * \brief Endpoint transfer direction is IN + */ +#define USB_EP_DIR_IN 0x80 + +/** + * \brief Endpoint transfer direction is OUT + */ +#define USB_EP_DIR_OUT 0x00 + +//! \brief Endpoint identifier +typedef uint8_t usb_ep_t; + +/** + * \brief Maximum length in bytes of a USB descriptor + * + * The maximum length of a USB descriptor is limited by the 8-bit + * bLength field. + */ +#define USB_MAX_DESC_LEN 255 + +/* + * 2-byte alignment requested for all USB structures. + */ +COMPILER_PACK_SET(1) + +/** + * \brief A USB Device SETUP request + * + * The data payload of SETUP packets always follows this structure. + */ +typedef struct { + uint8_t bmRequestType; + uint8_t bRequest; + le16_t wValue; + le16_t wIndex; + le16_t wLength; +} usb_setup_req_t; + +/** + * \brief Standard USB device descriptor structure + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + le16_t bcdUSB; + uint8_t bDeviceClass; + uint8_t bDeviceSubClass; + uint8_t bDeviceProtocol; + uint8_t bMaxPacketSize0; + le16_t idVendor; + le16_t idProduct; + le16_t bcdDevice; + uint8_t iManufacturer; + uint8_t iProduct; + uint8_t iSerialNumber; + uint8_t bNumConfigurations; +} usb_dev_desc_t; + +/** + * \brief Standard USB device qualifier descriptor structure + * + * This descriptor contains information about the device when running at + * the "other" speed (i.e. if the device is currently operating at high + * speed, this descriptor can be used to determine what would change if + * the device was operating at full speed.) + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + le16_t bcdUSB; + uint8_t bDeviceClass; + uint8_t bDeviceSubClass; + uint8_t bDeviceProtocol; + uint8_t bMaxPacketSize0; + uint8_t bNumConfigurations; + uint8_t bReserved; +} usb_dev_qual_desc_t; + +/** + * \brief USB Device BOS descriptor structure + * + * The BOS descriptor (Binary device Object Store) defines a root + * descriptor that is similar to the configuration descriptor, and is + * the base descriptor for accessing a family of related descriptors. + * A host can read a BOS descriptor and learn from the wTotalLength field + * the entire size of the device-level descriptor set, or it can read in + * the entire BOS descriptor set of device capabilities. + * The host accesses this descriptor using the GetDescriptor() request. + * The descriptor type in the GetDescriptor() request is set to BOS. + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + le16_t wTotalLength; + uint8_t bNumDeviceCaps; +} usb_dev_bos_desc_t; + + +/** + * \brief USB Device Capabilities - USB 2.0 Extension Descriptor structure + * + * Defines the set of USB 1.1-specific device level capabilities. + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bDevCapabilityType; + le32_t bmAttributes; +} usb_dev_capa_ext_desc_t; + +/** + * \brief USB Device LPM Descriptor structure + * + * The BOS descriptor and capabilities descriptors for LPM. + */ +typedef struct { + usb_dev_bos_desc_t bos; + usb_dev_capa_ext_desc_t capa_ext; +} usb_dev_lpm_desc_t; + +/** + * \brief Standard USB Interface Association Descriptor structure + */ +typedef struct { + uint8_t bLength; //!< size of this descriptor in bytes + uint8_t bDescriptorType; //!< INTERFACE descriptor type + uint8_t bFirstInterface; //!< Number of interface + uint8_t bInterfaceCount; //!< value to select alternate setting + uint8_t bFunctionClass; //!< Class code assigned by the USB + uint8_t bFunctionSubClass;//!< Sub-class code assigned by the USB + uint8_t bFunctionProtocol;//!< Protocol code assigned by the USB + uint8_t iFunction; //!< Index of string descriptor +} usb_association_desc_t; + + +/** + * \brief Standard USB configuration descriptor structure + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + le16_t wTotalLength; + uint8_t bNumInterfaces; + uint8_t bConfigurationValue; + uint8_t iConfiguration; + uint8_t bmAttributes; + uint8_t bMaxPower; +} usb_conf_desc_t; + + +#define USB_CONFIG_ATTR_MUST_SET (1 << 7) //!< Must always be set +#define USB_CONFIG_ATTR_BUS_POWERED (0 << 6) //!< Bus-powered +#define USB_CONFIG_ATTR_SELF_POWERED (1 << 6) //!< Self-powered +#define USB_CONFIG_ATTR_REMOTE_WAKEUP (1 << 5) //!< remote wakeup supported + +#define USB_CONFIG_MAX_POWER(ma) (((ma) + 1) / 2) //!< Max power in mA + +/** + * \brief Standard USB association descriptor structure + */ +typedef struct { + uint8_t bLength; //!< Size of this descriptor in bytes + uint8_t bDescriptorType; //!< Interface descriptor type + uint8_t bFirstInterface; //!< Number of interface + uint8_t bInterfaceCount; //!< value to select alternate setting + uint8_t bFunctionClass; //!< Class code assigned by the USB + uint8_t bFunctionSubClass; //!< Sub-class code assigned by the USB + uint8_t bFunctionProtocol; //!< Protocol code assigned by the USB + uint8_t iFunction; //!< Index of string descriptor +} usb_iad_desc_t; + +/** + * \brief Standard USB interface descriptor structure + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bInterfaceNumber; + uint8_t bAlternateSetting; + uint8_t bNumEndpoints; + uint8_t bInterfaceClass; + uint8_t bInterfaceSubClass; + uint8_t bInterfaceProtocol; + uint8_t iInterface; +} usb_iface_desc_t; + +/** + * \brief Standard USB endpoint descriptor structure + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; + uint8_t bEndpointAddress; + uint8_t bmAttributes; + le16_t wMaxPacketSize; + uint8_t bInterval; +} usb_ep_desc_t; + + +/** + * \brief A standard USB string descriptor structure + */ +typedef struct { + uint8_t bLength; + uint8_t bDescriptorType; +} usb_str_desc_t; + +typedef struct { + usb_str_desc_t desc; + le16_t string[1]; +} usb_str_lgid_desc_t; + +COMPILER_PACK_RESET() + +//! @} + +#endif /* _USB_PROTOCOL_H_ */ diff --git a/src/HAL/DUE/usb/usb_protocol_cdc.h b/src/HAL/DUE/usb/usb_protocol_cdc.h new file mode 100644 index 0000000..d594db5 --- /dev/null +++ b/src/HAL/DUE/usb/usb_protocol_cdc.h @@ -0,0 +1,320 @@ +/** + * \file + * + * \brief USB Communication Device Class (CDC) protocol definitions + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ +#ifndef _USB_PROTOCOL_CDC_H_ +#define _USB_PROTOCOL_CDC_H_ + +#include "compiler.h" + +/** + * \ingroup usb_protocol_group + * \defgroup cdc_protocol_group Communication Device Class Definitions + * @{ + */ + +/** + * \name Possible values of class + */ +//@{ +#define CDC_CLASS_DEVICE 0x02 //!< USB Communication Device Class +#define CDC_CLASS_COMM 0x02 //!< CDC Communication Class Interface +#define CDC_CLASS_DATA 0x0A //!< CDC Data Class Interface +#define CDC_CLASS_MULTI 0xEF //!< CDC Multi-interface Function + +//@} + +//! \name USB CDC Subclass IDs +//@{ +#define CDC_SUBCLASS_DLCM 0x01 //!< Direct Line Control Model +#define CDC_SUBCLASS_ACM 0x02 //!< Abstract Control Model +#define CDC_SUBCLASS_TCM 0x03 //!< Telephone Control Model +#define CDC_SUBCLASS_MCCM 0x04 //!< Multi-Channel Control Model +#define CDC_SUBCLASS_CCM 0x05 //!< CAPI Control Model +#define CDC_SUBCLASS_ETH 0x06 //!< Ethernet Networking Control Model +#define CDC_SUBCLASS_ATM 0x07 //!< ATM Networking Control Model +//@} + +//! \name USB CDC Communication Interface Protocol IDs +//@{ +#define CDC_PROTOCOL_V25TER 0x01 //!< Common AT commands +//@} + +//! \name USB CDC Data Interface Protocol IDs +//@{ +#define CDC_PROTOCOL_I430 0x30 //!< ISDN BRI +#define CDC_PROTOCOL_HDLC 0x31 //!< HDLC +#define CDC_PROTOCOL_TRANS 0x32 //!< Transparent +#define CDC_PROTOCOL_Q921M 0x50 //!< Q.921 management protocol +#define CDC_PROTOCOL_Q921 0x51 //!< Q.931 [sic] Data link protocol +#define CDC_PROTOCOL_Q921TM 0x52 //!< Q.921 TEI-multiplexor +#define CDC_PROTOCOL_V42BIS 0x90 //!< Data compression procedures +#define CDC_PROTOCOL_Q931 0x91 //!< Euro-ISDN protocol control +#define CDC_PROTOCOL_V120 0x92 //!< V.24 rate adaption to ISDN +#define CDC_PROTOCOL_CAPI20 0x93 //!< CAPI Commands +#define CDC_PROTOCOL_HOST 0xFD //!< Host based driver +/** + * \brief Describes the Protocol Unit Functional Descriptors [sic] + * on Communication Class Interface + */ +#define CDC_PROTOCOL_PUFD 0xFE +//@} + +//! \name USB CDC Functional Descriptor Types +//@{ +#define CDC_CS_INTERFACE 0x24 //!< Interface Functional Descriptor +#define CDC_CS_ENDPOINT 0x25 //!< Endpoint Functional Descriptor +//@} + +//! \name USB CDC Functional Descriptor Subtypes +//@{ +#define CDC_SCS_HEADER 0x00 //!< Header Functional Descriptor +#define CDC_SCS_CALL_MGMT 0x01 //!< Call Management +#define CDC_SCS_ACM 0x02 //!< Abstract Control Management +#define CDC_SCS_UNION 0x06 //!< Union Functional Descriptor +//@} + +//! \name USB CDC Request IDs +//@{ +#define USB_REQ_CDC_SEND_ENCAPSULATED_COMMAND 0x00 +#define USB_REQ_CDC_GET_ENCAPSULATED_RESPONSE 0x01 +#define USB_REQ_CDC_SET_COMM_FEATURE 0x02 +#define USB_REQ_CDC_GET_COMM_FEATURE 0x03 +#define USB_REQ_CDC_CLEAR_COMM_FEATURE 0x04 +#define USB_REQ_CDC_SET_AUX_LINE_STATE 0x10 +#define USB_REQ_CDC_SET_HOOK_STATE 0x11 +#define USB_REQ_CDC_PULSE_SETUP 0x12 +#define USB_REQ_CDC_SEND_PULSE 0x13 +#define USB_REQ_CDC_SET_PULSE_TIME 0x14 +#define USB_REQ_CDC_RING_AUX_JACK 0x15 +#define USB_REQ_CDC_SET_LINE_CODING 0x20 +#define USB_REQ_CDC_GET_LINE_CODING 0x21 +#define USB_REQ_CDC_SET_CONTROL_LINE_STATE 0x22 +#define USB_REQ_CDC_SEND_BREAK 0x23 +#define USB_REQ_CDC_SET_RINGER_PARMS 0x30 +#define USB_REQ_CDC_GET_RINGER_PARMS 0x31 +#define USB_REQ_CDC_SET_OPERATION_PARMS 0x32 +#define USB_REQ_CDC_GET_OPERATION_PARMS 0x33 +#define USB_REQ_CDC_SET_LINE_PARMS 0x34 +#define USB_REQ_CDC_GET_LINE_PARMS 0x35 +#define USB_REQ_CDC_DIAL_DIGITS 0x36 +#define USB_REQ_CDC_SET_UNIT_PARAMETER 0x37 +#define USB_REQ_CDC_GET_UNIT_PARAMETER 0x38 +#define USB_REQ_CDC_CLEAR_UNIT_PARAMETER 0x39 +#define USB_REQ_CDC_GET_PROFILE 0x3A +#define USB_REQ_CDC_SET_ETHERNET_MULTICAST_FILTERS 0x40 +#define USB_REQ_CDC_SET_ETHERNET_POWER_MANAGEMENT_PATTERNFILTER 0x41 +#define USB_REQ_CDC_GET_ETHERNET_POWER_MANAGEMENT_PATTERNFILTER 0x42 +#define USB_REQ_CDC_SET_ETHERNET_PACKET_FILTER 0x43 +#define USB_REQ_CDC_GET_ETHERNET_STATISTIC 0x44 +#define USB_REQ_CDC_SET_ATM_DATA_FORMAT 0x50 +#define USB_REQ_CDC_GET_ATM_DEVICE_STATISTICS 0x51 +#define USB_REQ_CDC_SET_ATM_DEFAULT_VC 0x52 +#define USB_REQ_CDC_GET_ATM_VC_STATISTICS 0x53 +// Added bNotification codes according cdc spec 1.1 chapter 6.3 +#define USB_REQ_CDC_NOTIFY_RING_DETECT 0x09 +#define USB_REQ_CDC_NOTIFY_SERIAL_STATE 0x20 +#define USB_REQ_CDC_NOTIFY_CALL_STATE_CHANGE 0x28 +#define USB_REQ_CDC_NOTIFY_LINE_STATE_CHANGE 0x29 +//@} + +/* + * Need to pack structures tightly, or the compiler might insert padding + * and violate the spec-mandated layout. + */ +COMPILER_PACK_SET(1) + +//! \name USB CDC Descriptors +//@{ + + +//! CDC Header Functional Descriptor +typedef struct { + uint8_t bFunctionLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + le16_t bcdCDC; +} usb_cdc_hdr_desc_t; + +//! CDC Call Management Functional Descriptor +typedef struct { + uint8_t bFunctionLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bmCapabilities; + uint8_t bDataInterface; +} usb_cdc_call_mgmt_desc_t; + +//! CDC ACM Functional Descriptor +typedef struct { + uint8_t bFunctionLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bmCapabilities; +} usb_cdc_acm_desc_t; + +//! CDC Union Functional Descriptor +typedef struct { + uint8_t bFunctionLength; + uint8_t bDescriptorType; + uint8_t bDescriptorSubtype; + uint8_t bMasterInterface; + uint8_t bSlaveInterface0; +} usb_cdc_union_desc_t; + + +//! \name USB CDC Call Management Capabilities +//@{ +//! Device handles call management itself +#define CDC_CALL_MGMT_SUPPORTED (1 << 0) +//! Device can send/receive call management info over a Data Class interface +#define CDC_CALL_MGMT_OVER_DCI (1 << 1) +//@} + +//! \name USB CDC ACM Capabilities +//@{ +//! Device supports the request combination of +//! Set_Comm_Feature, Clear_Comm_Feature, and Get_Comm_Feature. +#define CDC_ACM_SUPPORT_FEATURE_REQUESTS (1 << 0) +//! Device supports the request combination of +//! Set_Line_Coding, Set_Control_Line_State, Get_Line_Coding, +//! and the notification Serial_State. +#define CDC_ACM_SUPPORT_LINE_REQUESTS (1 << 1) +//! Device supports the request Send_Break +#define CDC_ACM_SUPPORT_SENDBREAK_REQUESTS (1 << 2) +//! Device supports the notification Network_Connection. +#define CDC_ACM_SUPPORT_NOTIFY_REQUESTS (1 << 3) +//@} +//@} + +//! \name USB CDC line control +//@{ + +//! \name USB CDC line coding +//@{ +//! Line Coding structure +typedef struct { + le32_t dwDTERate; + uint8_t bCharFormat; + uint8_t bParityType; + uint8_t bDataBits; +} usb_cdc_line_coding_t; +//! Possible values of bCharFormat +enum cdc_char_format { + CDC_STOP_BITS_1 = 0, //!< 1 stop bit + CDC_STOP_BITS_1_5 = 1, //!< 1.5 stop bits + CDC_STOP_BITS_2 = 2, //!< 2 stop bits +}; +//! Possible values of bParityType +enum cdc_parity { + CDC_PAR_NONE = 0, //!< No parity + CDC_PAR_ODD = 1, //!< Odd parity + CDC_PAR_EVEN = 2, //!< Even parity + CDC_PAR_MARK = 3, //!< Parity forced to 0 (space) + CDC_PAR_SPACE = 4, //!< Parity forced to 1 (mark) +}; +//@} + +//! \name USB CDC control signals +//! spec 1.1 chapter 6.2.14 +//@{ + +//! Control signal structure +typedef struct { + uint16_t value; +} usb_cdc_control_signal_t; + +//! \name Possible values in usb_cdc_control_signal_t +//@{ +//! Carrier control for half duplex modems. +//! This signal corresponds to V.24 signal 105 and RS-232 signal RTS. +//! The device ignores the value of this bit +//! when operating in full duplex mode. +#define CDC_CTRL_SIGNAL_ACTIVATE_CARRIER (1 << 1) +//! Indicates to DCE if DTE is present or not. +//! This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR. +#define CDC_CTRL_SIGNAL_DTE_PRESENT (1 << 0) +//@} +//@} + + +//! \name USB CDC notification message +//@{ + +typedef struct { + uint8_t bmRequestType; + uint8_t bNotification; + le16_t wValue; + le16_t wIndex; + le16_t wLength; +} usb_cdc_notify_msg_t; + +//! \name USB CDC serial state +//@{* + +//! Hardware handshake support (cdc spec 1.1 chapter 6.3.5) +typedef struct { + usb_cdc_notify_msg_t header; + le16_t value; +} usb_cdc_notify_serial_state_t; + +//! \name Possible values in usb_cdc_notify_serial_state_t +//@{ +#define CDC_SERIAL_STATE_DCD CPU_TO_LE16((1<<0)) +#define CDC_SERIAL_STATE_DSR CPU_TO_LE16((1<<1)) +#define CDC_SERIAL_STATE_BREAK CPU_TO_LE16((1<<2)) +#define CDC_SERIAL_STATE_RING CPU_TO_LE16((1<<3)) +#define CDC_SERIAL_STATE_FRAMING CPU_TO_LE16((1<<4)) +#define CDC_SERIAL_STATE_PARITY CPU_TO_LE16((1<<5)) +#define CDC_SERIAL_STATE_OVERRUN CPU_TO_LE16((1<<6)) +//@} +//! @} + +//! @} + +COMPILER_PACK_RESET() + +//! @} + +#endif // _USB_PROTOCOL_CDC_H_ diff --git a/src/HAL/DUE/usb/usb_protocol_msc.h b/src/HAL/DUE/usb/usb_protocol_msc.h new file mode 100644 index 0000000..e1e5923 --- /dev/null +++ b/src/HAL/DUE/usb/usb_protocol_msc.h @@ -0,0 +1,147 @@ +/** + * \file + * + * \brief USB Mass Storage Class (MSC) protocol definitions. + * + * Copyright (c) 2009-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _USB_PROTOCOL_MSC_H_ +#define _USB_PROTOCOL_MSC_H_ + + +/** + * \ingroup usb_protocol_group + * \defgroup usb_msc_protocol USB Mass Storage Class (MSC) protocol definitions + * + * @{ + */ + +/** + * \name Possible Class value + */ +//@{ +#define MSC_CLASS 0x08 +//@} + +/** + * \name Possible SubClass value + * \note In practise, most devices should use + * #MSC_SUBCLASS_TRANSPARENT and specify the actual command set in + * the standard INQUIRY data block, even if the MSC spec indicates + * otherwise. In particular, RBC is not supported by certain major + * operating systems like Windows XP. + */ +//@{ +#define MSC_SUBCLASS_RBC 0x01 //!< Reduced Block Commands +#define MSC_SUBCLASS_ATAPI 0x02 //!< CD/DVD devices +#define MSC_SUBCLASS_QIC_157 0x03 //!< Tape devices +#define MSC_SUBCLASS_UFI 0x04 //!< Floppy disk drives +#define MSC_SUBCLASS_SFF_8070I 0x05 //!< Floppy disk drives +#define MSC_SUBCLASS_TRANSPARENT 0x06 //!< Determined by INQUIRY +//@} + +/** + * \name Possible protocol value + * \note Only the BULK protocol should be used in new designs. + */ +//@{ +#define MSC_PROTOCOL_CBI 0x00 //!< Command/Bulk/Interrupt +#define MSC_PROTOCOL_CBI_ALT 0x01 //!< W/o command completion +#define MSC_PROTOCOL_BULK 0x50 //!< Bulk-only +//@} + + +/** + * \brief MSC USB requests (bRequest) + */ +enum usb_reqid_msc { + USB_REQ_MSC_BULK_RESET = 0xFF, //!< Mass Storage Reset + USB_REQ_MSC_GET_MAX_LUN = 0xFE //!< Get Max LUN +}; + + +COMPILER_PACK_SET(1) + +/** + * \name A Command Block Wrapper (CBW). + */ +//@{ +struct usb_msc_cbw { + le32_t dCBWSignature; //!< Must contain 'USBC' + le32_t dCBWTag; //!< Unique command ID + le32_t dCBWDataTransferLength; //!< Number of bytes to transfer + uint8_t bmCBWFlags; //!< Direction in bit 7 + uint8_t bCBWLUN; //!< Logical Unit Number + uint8_t bCBWCBLength; //!< Number of valid CDB bytes + uint8_t CDB[16]; //!< SCSI Command Descriptor Block +}; + +#define USB_CBW_SIGNATURE 0x55534243 //!< dCBWSignature value +#define USB_CBW_DIRECTION_IN (1<<7) //!< Data from device to host +#define USB_CBW_DIRECTION_OUT (0<<7) //!< Data from host to device +#define USB_CBW_LUN_MASK 0x0F //!< Valid bits in bCBWLUN +#define USB_CBW_LEN_MASK 0x1F //!< Valid bits in bCBWCBLength +//@} + + +/** + * \name A Command Status Wrapper (CSW). + */ +//@{ +struct usb_msc_csw { + le32_t dCSWSignature; //!< Must contain 'USBS' + le32_t dCSWTag; //!< Same as dCBWTag + le32_t dCSWDataResidue; //!< Number of bytes not transferred + uint8_t bCSWStatus; //!< Status code +}; + +#define USB_CSW_SIGNATURE 0x55534253 //!< dCSWSignature value +#define USB_CSW_STATUS_PASS 0x00 //!< Command Passed +#define USB_CSW_STATUS_FAIL 0x01 //!< Command Failed +#define USB_CSW_STATUS_PE 0x02 //!< Phase Error +//@} + +COMPILER_PACK_RESET() + +//@} + +#endif // _USB_PROTOCOL_MSC_H_ diff --git a/src/HAL/DUE/usb/usb_task.c b/src/HAL/DUE/usb/usb_task.c new file mode 100644 index 0000000..54a808d --- /dev/null +++ b/src/HAL/DUE/usb/usb_task.c @@ -0,0 +1,341 @@ +/** + * \file + * + * \brief Main functions for USB composite example + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ + +// Support and FAQ: visit Atmel Support + +#ifdef ARDUINO_ARCH_SAM + +#include +#include + +#include "conf_usb.h" +#include "udc.h" + +#if ENABLED(SDSUPPORT) + static volatile bool main_b_msc_enable = false; +#endif +static volatile bool main_b_cdc_enable = false; +static volatile bool main_b_dtr_active = false; + +void usb_task_idle(void) { + #if ENABLED(SDSUPPORT) + // Attend SD card access from the USB MSD -- Prioritize access to improve speed + int delay = 2; + while (main_b_msc_enable && --delay > 0) { + if (udi_msc_process_trans()) delay = 10000; + + // Reset the watchdog, just to be sure + REG_WDT_CR = WDT_CR_WDRSTT | WDT_CR_KEY(0xA5); + } + #endif +} + +#if ENABLED(SDSUPPORT) + bool usb_task_msc_enable(void) { return ((main_b_msc_enable = true)); } + void usb_task_msc_disable(void) { main_b_msc_enable = false; } + bool usb_task_msc_isenabled(void) { return main_b_msc_enable; } +#endif + +bool usb_task_cdc_enable(const uint8_t port) { UNUSED(port); return ((main_b_cdc_enable = true)); } +void usb_task_cdc_disable(const uint8_t port) { UNUSED(port); main_b_cdc_enable = false; main_b_dtr_active = false; } +bool usb_task_cdc_isenabled(void) { return main_b_cdc_enable; } + +/*! \brief Called by CDC interface + * Callback running when CDC device have received data + */ +void usb_task_cdc_rx_notify(const uint8_t port) { UNUSED(port); } + +/*! \brief Configures communication line + * + * \param cfg line configuration + */ +static uint16_t dwDTERate = 0; +void usb_task_cdc_config(const uint8_t port, usb_cdc_line_coding_t *cfg) { + UNUSED(port); + // Store last DTE rate + dwDTERate = cfg->dwDTERate; +} + +void usb_task_cdc_set_dtr(const uint8_t port, const bool b_enable) { + UNUSED(port); + // Keep DTR status + main_b_dtr_active = b_enable; + + // Implement Arduino-Compatible kludge to enter programming mode from + // the native port: + // "Auto-reset into the bootloader is triggered when the port, already + // open at 1200 bps, is closed." + + if (1200 == dwDTERate) { + // We check DTR state to determine if host port is open (bit 0 of lineState). + if (!b_enable) { + + // Set RST pin to go low for 65535 clock cycles on reset + // This helps restarting when firmware flash ends + RSTC->RSTC_MR = 0xA5000F01; + + // Schedule delayed reset + initiateReset(250); + } + else + cancelReset(); + } +} + +bool usb_task_cdc_dtr_active(void) { return main_b_dtr_active; } + +/// Microsoft WCID descriptor +typedef struct USB_MicrosoftCompatibleDescriptor_Interface { + uint8_t bFirstInterfaceNumber; + uint8_t reserved1; + uint8_t compatibleID[8]; + uint8_t subCompatibleID[8]; + uint8_t reserved2[6]; +} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor_Interface; + +typedef struct USB_MicrosoftCompatibleDescriptor { + uint32_t dwLength; + uint16_t bcdVersion; + uint16_t wIndex; + uint8_t bCount; + uint8_t reserved[7]; + USB_MicrosoftCompatibleDescriptor_Interface interfaces[]; +} __attribute__((packed)) USB_MicrosoftCompatibleDescriptor; + +// 3D Printer compatible descriptor +static USB_MicrosoftCompatibleDescriptor microsoft_compatible_id_descriptor = { + .dwLength = sizeof(USB_MicrosoftCompatibleDescriptor) + + 1*sizeof(USB_MicrosoftCompatibleDescriptor_Interface), + .bcdVersion = 0x0100, + .wIndex = 0x0004, + .bCount = 1, + .reserved = {0, 0, 0, 0, 0, 0, 0}, + .interfaces = { + { + .bFirstInterfaceNumber = 0, + .reserved1 = 1, + .compatibleID = "3DPRINT", + .subCompatibleID = {0, 0, 0, 0, 0, 0, 0, 0}, + .reserved2 = {0, 0, 0, 0, 0, 0}, + } + } +}; + +#define xstr(s) str(s) +#define str(s) #s + +#define MS3DPRINT_CONFIG u"MS3DPrintConfig" +#define MS3DPRINT_CONFIG_DATA \ + u"Base=SD\0"\ + u"Job3DOutputAreaWidth=" xstr(X_BED_SIZE) "000\0"\ + u"Job3DOutputAreaDepth=" xstr(Y_BED_SIZE) "000\0"\ + u"Job3DOutputAreaHeight=" xstr(Z_MAX_POS) "000\0"\ + u"filamentdiameter=1750\0" + +typedef struct USB_MicrosoftExtendedPropertiesDescriptor { + uint32_t dwLength; + uint16_t bcdVersion; + uint16_t wIndex; + uint16_t bCount; + uint32_t dwPropertySize; + uint32_t dwPropertyDataType; + uint16_t wPropertyNameLength; + uint16_t PropertyName[sizeof(MS3DPRINT_CONFIG)/sizeof(uint16_t)]; + uint32_t dwPropertyDataLength; + uint16_t PropertyData[sizeof(MS3DPRINT_CONFIG_DATA)/sizeof(uint16_t)]; +} __attribute__((packed)) USB_MicrosoftExtendedPropertiesDescriptor; + +static USB_MicrosoftExtendedPropertiesDescriptor microsoft_extended_properties_descriptor = { + .dwLength = sizeof(USB_MicrosoftExtendedPropertiesDescriptor), + .bcdVersion = 0x0100, + .wIndex = 0x0005, + .bCount = 1, + + .dwPropertySize = 4 + 4 + 2 + 4 + sizeof(MS3DPRINT_CONFIG) + sizeof(MS3DPRINT_CONFIG_DATA), + .dwPropertyDataType = 7, // (1=REG_SZ, 4=REG_DWORD, 7=REG_MULTI_SZ) + .wPropertyNameLength = sizeof(MS3DPRINT_CONFIG), + .PropertyName = MS3DPRINT_CONFIG, + .dwPropertyDataLength = sizeof(MS3DPRINT_CONFIG_DATA), + .PropertyData = MS3DPRINT_CONFIG_DATA +}; + +/************************************************************************************************** +** WCID configuration information +** Hooked into UDC via UDC_GET_EXTRA_STRING #define. +*/ +bool usb_task_extra_string(void) { + static uint8_t udi_msft_magic[] = "MSFT100\xEE"; + static uint8_t udi_cdc_name[] = "CDC interface"; + #if ENABLED(SDSUPPORT) + static uint8_t udi_msc_name[] = "MSC interface"; + #endif + + struct extra_strings_desc_t { + usb_str_desc_t header; + #if ENABLED(SDSUPPORT) + le16_t string[Max(Max(sizeof(udi_cdc_name) - 1, sizeof(udi_msc_name) - 1), sizeof(udi_msft_magic) - 1)]; + #else + le16_t string[Max(sizeof(udi_cdc_name) - 1, sizeof(udi_msft_magic) - 1)]; + #endif + }; + static UDC_DESC_STORAGE struct extra_strings_desc_t extra_strings_desc = { + .header.bDescriptorType = USB_DT_STRING + }; + + uint8_t *str; + uint8_t str_lgt = 0; + + // Link payload pointer to the string corresponding at request + switch (udd_g_ctrlreq.req.wValue & 0xFF) { + case UDI_CDC_IAD_STRING_ID: + str_lgt = sizeof(udi_cdc_name) - 1; + str = udi_cdc_name; + break; + #if ENABLED(SDSUPPORT) + case UDI_MSC_STRING_ID: + str_lgt = sizeof(udi_msc_name) - 1; + str = udi_msc_name; + break; + #endif + case 0xEE: + str_lgt = sizeof(udi_msft_magic) - 1; + str = udi_msft_magic; + break; + default: + return false; + } + + for (uint8_t i = 0; i < str_lgt; i++) + extra_strings_desc.string[i] = cpu_to_le16((le16_t)str[i]); + + extra_strings_desc.header.bLength = 2 + str_lgt * 2; + udd_g_ctrlreq.payload_size = extra_strings_desc.header.bLength; + udd_g_ctrlreq.payload = (uint8_t*)&extra_strings_desc; + + // if the string is larger than request length, then cut it + if (udd_g_ctrlreq.payload_size > udd_g_ctrlreq.req.wLength) { + udd_g_ctrlreq.payload_size = udd_g_ctrlreq.req.wLength; + } + + return true; +} + +/************************************************************************************************** +** Handle device requests that the ASF stack doesn't +*/ +bool usb_task_other_requests(void) { + uint8_t *ptr = 0; + uint16_t size = 0; + + if (Udd_setup_type() == USB_REQ_TYPE_VENDOR) { + //if (udd_g_ctrlreq.req.bRequest == 0x30) + if (1) { + if (udd_g_ctrlreq.req.wIndex == 0x04) { + ptr = (uint8_t*)µsoft_compatible_id_descriptor; + size = (udd_g_ctrlreq.req.wLength); + if (size > microsoft_compatible_id_descriptor.dwLength) + size = microsoft_compatible_id_descriptor.dwLength; + } + else if (udd_g_ctrlreq.req.wIndex == 0x05) { + ptr = (uint8_t*)µsoft_extended_properties_descriptor; + size = (udd_g_ctrlreq.req.wLength); + if (size > microsoft_extended_properties_descriptor.dwLength) + size = microsoft_extended_properties_descriptor.dwLength; + } + else + return false; + } + } + + udd_g_ctrlreq.payload_size = size; + if (size == 0) { + udd_g_ctrlreq.callback = 0; + udd_g_ctrlreq.over_under_run = 0; + } + else + udd_g_ctrlreq.payload = ptr; + + return true; +} + +void usb_task_init(void) { + + uint16_t *ptr; + + // Disable USB peripheral so we start clean and avoid lockups + otg_disable(); + udd_disable(); + + // Set the USB interrupt to our stack + UDD_SetStack(&USBD_ISR); + + // Start USB stack to authorize VBus monitoring + udc_start(); + + // Patch in filament diameter - Be careful: String is in UNICODE (2bytes per char) + ptr = µsoft_extended_properties_descriptor.PropertyData[0]; + while (ptr[0] || ptr[1]) { // Double 0 flags end of resource + + // Found the filamentdiameter= unicode string + if (ptr[0] == 'r' && ptr[1] == '=') { + char diam[16]; + char *sptr; + + // Patch in the filament diameter + itoa((int)((DEFAULT_NOMINAL_FILAMENT_DIA) * 1000), diam, 10); + + // And copy it to the proper place, expanding it to unicode + sptr = &diam[0]; + ptr += 2; + while (*sptr) *ptr++ = *sptr++; + + // Done! + break; + } + + // Go to the next character + ptr++; + } +} + +#endif // ARDUINO_ARCH_SAM diff --git a/src/HAL/DUE/usb/usb_task.h b/src/HAL/DUE/usb/usb_task.h new file mode 100644 index 0000000..e9831ae --- /dev/null +++ b/src/HAL/DUE/usb/usb_task.h @@ -0,0 +1,134 @@ +/** + * \file + * + * \brief Declaration of main function used by Composite example 4 + * + * Copyright (c) 2011-2015 Atmel Corporation. All rights reserved. + * + * \asf_license_start + * + * \page License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. The name of Atmel may not be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * 4. This software may only be redistributed and used in connection with an + * Atmel microcontroller product. + * + * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE + * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \asf_license_stop + * + */ +/* + * Support and FAQ: visit Atmel Support + */ + +#ifndef _USB_TASK_H_ +#define _USB_TASK_H_ + +#include "usb_protocol_cdc.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! \brief Called by MSC interface + * Callback running when USB Host enable MSC interface + * + * \retval true if MSC startup is ok + */ +bool usb_task_msc_enable(void); + +/*! \brief Called by MSC interface + * Callback running when USB Host disable MSC interface + */ +void usb_task_msc_disable(void); + +/*! \brief Opens the communication port + * This is called by CDC interface when USB Host enable it. + * + * \retval true if cdc startup is successfully done + */ +bool usb_task_cdc_enable(const uint8_t port); + +/*! \brief Closes the communication port + * This is called by CDC interface when USB Host disable it. + */ +void usb_task_cdc_disable(const uint8_t port); + +/*! \brief Save new DTR state to change led behavior. + * The DTR notify that the terminal have open or close the communication port. + */ +void usb_task_cdc_set_dtr(const uint8_t port, const bool b_enable); + +/*! \brief Check if MSC is enumerated and configured on the PC side + */ +bool usb_task_msc_isenabled(void); + +/*! \brief Check if CDC is enumerated and configured on the PC side + */ +bool usb_task_cdc_isenabled(void); + +/*! \brief Check if CDC is actually OPEN by an application on the PC side + * assuming DTR signal means a program is listening to messages + */ +bool usb_task_cdc_dtr_active(void); + +/*! \brief Called by UDC when USB Host request a extra string different + * of this specified in USB device descriptor + */ +bool usb_task_extra_string(void); + +/*! \brief Called by UDC when USB Host performs unknown requests + */ +bool usb_task_other_requests(void); + +/*! \brief Called by CDC interface + * Callback running when CDC device have received data + */ +void usb_task_cdc_rx_notify(const uint8_t port); + +/*! \brief Configures communication line + * + * \param cfg line configuration + */ +void usb_task_cdc_config(const uint8_t port, usb_cdc_line_coding_t *cfg); + +/*! \brief The USB device interrupt + */ +void USBD_ISR(void); + +/*! \brief USB task init + */ +void usb_task_init(void); + +/*! \brief USB task idle + */ +void usb_task_idle(void); + +#ifdef __cplusplus +} +#endif + +#endif // _USB_TASK_H_ diff --git a/src/HAL/ESP32/FlushableHardwareSerial.cpp b/src/HAL/ESP32/FlushableHardwareSerial.cpp new file mode 100644 index 0000000..1456622 --- /dev/null +++ b/src/HAL/ESP32/FlushableHardwareSerial.cpp @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef ARDUINO_ARCH_ESP32 + +#include "FlushableHardwareSerial.h" + +Serial1Class flushableSerial(false, 0); + +#endif diff --git a/src/HAL/ESP32/FlushableHardwareSerial.h b/src/HAL/ESP32/FlushableHardwareSerial.h new file mode 100644 index 0000000..012dda8 --- /dev/null +++ b/src/HAL/ESP32/FlushableHardwareSerial.h @@ -0,0 +1,34 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +#include "../shared/Marduino.h" +#include "../../core/serial_hook.h" + +class FlushableHardwareSerial : public HardwareSerial { +public: + FlushableHardwareSerial(int uart_nr) : HardwareSerial(uart_nr) {} +}; + +extern Serial1Class flushableSerial; diff --git a/src/HAL/ESP32/HAL.cpp b/src/HAL/ESP32/HAL.cpp new file mode 100644 index 0000000..29f3be3 --- /dev/null +++ b/src/HAL/ESP32/HAL.cpp @@ -0,0 +1,429 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" + +#include +#include +#include +#include + +#if ENABLED(USE_ESP32_TASK_WDT) + #include +#endif + +#if ENABLED(WIFISUPPORT) + #include + #include "wifi.h" + #if ENABLED(OTASUPPORT) + #include "ota.h" + #endif + #if ENABLED(WEBSUPPORT) + #include "spiffs.h" + #include "web.h" + #endif +#endif + +#if ENABLED(ESP3D_WIFISUPPORT) + DefaultSerial1 MSerial0(false, Serial2Socket); +#endif + +// ------------------------ +// Externs +// ------------------------ + +portMUX_TYPE MarlinHAL::spinlock = portMUX_INITIALIZER_UNLOCKED; + +// ------------------------ +// Local defines +// ------------------------ + +#define V_REF 1100 + +// ------------------------ +// Public Variables +// ------------------------ + +uint16_t MarlinHAL::adc_result; +pwm_pin_t MarlinHAL::pwm_pin_data[MAX_EXPANDER_BITS]; + +// ------------------------ +// Private Variables +// ------------------------ + +esp_adc_cal_characteristics_t characteristics[ADC_ATTEN_MAX]; +adc_atten_t attenuations[ADC1_CHANNEL_MAX] = {}; +uint32_t thresholds[ADC_ATTEN_MAX]; + +volatile int numPWMUsed = 0; +volatile struct { pin_t pin; int value; } pwmState[MAX_PWM_PINS]; + +pin_t chan_pin[CHANNEL_MAX_NUM + 1] = { 0 }; // PWM capable IOpins - not 0 or >33 on ESP32 + +struct { + uint32_t freq; // ledcReadFreq doesn't work if a duty hasn't been set yet! + uint16_t res; +} pwmInfo[(CHANNEL_MAX_NUM + 1) / 2]; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(WIFI_CUSTOM_COMMAND) + + bool wifi_custom_command(char * const command_ptr) { + #if ENABLED(ESP3D_WIFISUPPORT) + return esp3dlib.parse(command_ptr); + #else + UNUSED(command_ptr); + return false; + #endif + } + +#endif + +#if ENABLED(USE_ESP32_EXIO) + + HardwareSerial YSerial2(2); + + void Write_EXIO(uint8_t IO, uint8_t v) { + if (hal.isr_state()) { + hal.isr_off(); + YSerial2.write(0x80 | (((char)v) << 5) | (IO - 100)); + hal.isr_on(); + } + else + YSerial2.write(0x80 | (((char)v) << 5) | (IO - 100)); + } + +#endif + +void MarlinHAL::init_board() { + #if ENABLED(USE_ESP32_TASK_WDT) + esp_task_wdt_init(10, true); + #endif + #if ENABLED(ESP3D_WIFISUPPORT) + esp3dlib.init(); + #elif ENABLED(WIFISUPPORT) + wifi_init(); + TERN_(OTASUPPORT, OTA_init()); + #if ENABLED(WEBSUPPORT) + spiffs_init(); + web_init(); + #endif + server.begin(); + #endif + + // ESP32 uses a GPIO matrix that allows pins to be assigned to hardware serial ports. + // The following code initializes hardware Serial1 and Serial2 to use user-defined pins + // if they have been defined. + #if defined(HARDWARE_SERIAL1_RX) && defined(HARDWARE_SERIAL1_TX) + HardwareSerial Serial1(1); + #ifdef TMC_BAUD_RATE // use TMC_BAUD_RATE for Serial1 if defined + Serial1.begin(TMC_BAUD_RATE, SERIAL_8N1, HARDWARE_SERIAL1_RX, HARDWARE_SERIAL1_TX); + #else // use default BAUDRATE if TMC_BAUD_RATE not defined + Serial1.begin(BAUDRATE, SERIAL_8N1, HARDWARE_SERIAL1_RX, HARDWARE_SERIAL1_TX); + #endif + #endif + #if defined(HARDWARE_SERIAL2_RX) && defined(HARDWARE_SERIAL2_TX) + HardwareSerial Serial2(2); + #ifdef TMC_BAUD_RATE // use TMC_BAUD_RATE for Serial1 if defined + Serial2.begin(TMC_BAUD_RATE, SERIAL_8N1, HARDWARE_SERIAL2_RX, HARDWARE_SERIAL2_TX); + #else // use default BAUDRATE if TMC_BAUD_RATE not defined + Serial2.begin(BAUDRATE, SERIAL_8N1, HARDWARE_SERIAL2_RX, HARDWARE_SERIAL2_TX); + #endif + #endif + + // Initialize the i2s peripheral only if the I2S stepper stream is enabled. + // The following initialization is performed after Serial1 and Serial2 are defined as + // their native pins might conflict with the i2s stream even when they are remapped. + #if ENABLED(USE_ESP32_EXIO) + YSerial2.begin(460800 * 3, SERIAL_8N1, 16, 17); + #elif ENABLED(I2S_STEPPER_STREAM) + i2s_init(); + #endif +} + +void MarlinHAL::idletask() { + #if BOTH(WIFISUPPORT, OTASUPPORT) + OTA_handle(); + #endif + TERN_(ESP3D_WIFISUPPORT, esp3dlib.idletask()); +} + +uint8_t MarlinHAL::get_reset_source() { return rtc_get_reset_reason(1); } + +void MarlinHAL::reboot() { ESP.restart(); } + +void _delay_ms(int delay_ms) { delay(delay_ms); } + +// return free memory between end of heap (or end bss) and whatever is current +int MarlinHAL::freeMemory() { return ESP.getFreeHeap(); } + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout + + extern "C" { + esp_err_t esp_task_wdt_reset(); + } + + void watchdogSetup() { + // do whatever. don't remove this function. + } + + void MarlinHAL::watchdog_init() { + // TODO + } + + // Reset watchdog. + void MarlinHAL::watchdog_refresh() { esp_task_wdt_reset(); } + +#endif + +// ------------------------ +// ADC +// ------------------------ + +#define ADC1_CHANNEL(pin) ADC1_GPIO ## pin ## _CHANNEL + +adc1_channel_t get_channel(int pin) { + switch (pin) { + case 39: return ADC1_CHANNEL(39); + case 36: return ADC1_CHANNEL(36); + case 35: return ADC1_CHANNEL(35); + case 34: return ADC1_CHANNEL(34); + case 33: return ADC1_CHANNEL(33); + case 32: return ADC1_CHANNEL(32); + } + return ADC1_CHANNEL_MAX; +} + +void adc1_set_attenuation(adc1_channel_t chan, adc_atten_t atten) { + if (attenuations[chan] != atten) { + adc1_config_channel_atten(chan, atten); + attenuations[chan] = atten; + } +} + +void MarlinHAL::adc_init() { + // Configure ADC + adc1_config_width(ADC_WIDTH_12Bit); + + // Configure channels only if used as (re-)configuring a pin for ADC that is used elsewhere might have adverse effects + TERN_(HAS_TEMP_ADC_0, adc1_set_attenuation(get_channel(TEMP_0_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_1, adc1_set_attenuation(get_channel(TEMP_1_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_2, adc1_set_attenuation(get_channel(TEMP_2_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_3, adc1_set_attenuation(get_channel(TEMP_3_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_4, adc1_set_attenuation(get_channel(TEMP_4_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_5, adc1_set_attenuation(get_channel(TEMP_5_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_6, adc2_set_attenuation(get_channel(TEMP_6_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_ADC_7, adc3_set_attenuation(get_channel(TEMP_7_PIN), ADC_ATTEN_11db)); + TERN_(HAS_HEATED_BED, adc1_set_attenuation(get_channel(TEMP_BED_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_CHAMBER, adc1_set_attenuation(get_channel(TEMP_CHAMBER_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_PROBE, adc1_set_attenuation(get_channel(TEMP_PROBE_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_COOLER, adc1_set_attenuation(get_channel(TEMP_COOLER_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_BOARD, adc1_set_attenuation(get_channel(TEMP_BOARD_PIN), ADC_ATTEN_11db)); + TERN_(FILAMENT_WIDTH_SENSOR, adc1_set_attenuation(get_channel(FILWIDTH_PIN), ADC_ATTEN_11db)); + + // Note that adc2 is shared with the WiFi module, which has higher priority, so the conversion may fail. + // That's why we're not setting it up here. + + // Calculate ADC characteristics (i.e., gain and offset factors for each attenuation level) + for (int i = 0; i < ADC_ATTEN_MAX; i++) { + esp_adc_cal_characterize(ADC_UNIT_1, (adc_atten_t)i, ADC_WIDTH_BIT_12, V_REF, &characteristics[i]); + + // Change attenuation 100mV below the calibrated threshold + thresholds[i] = esp_adc_cal_raw_to_voltage(4095, &characteristics[i]); + } +} + +#ifndef ADC_REFERENCE_VOLTAGE + #define ADC_REFERENCE_VOLTAGE 3.3 +#endif + +void MarlinHAL::adc_start(const pin_t pin) { + const adc1_channel_t chan = get_channel(pin); + uint32_t mv; + esp_adc_cal_get_voltage((adc_channel_t)chan, &characteristics[attenuations[chan]], &mv); + + adc_result = mv * isr_float_t(1023) / isr_float_t(ADC_REFERENCE_VOLTAGE) / isr_float_t(1000); + + // Change the attenuation level based on the new reading + adc_atten_t atten; + if (mv < thresholds[ADC_ATTEN_DB_0] - 100) + atten = ADC_ATTEN_DB_0; + else if (mv > thresholds[ADC_ATTEN_DB_0] - 50 && mv < thresholds[ADC_ATTEN_DB_2_5] - 100) + atten = ADC_ATTEN_DB_2_5; + else if (mv > thresholds[ADC_ATTEN_DB_2_5] - 50 && mv < thresholds[ADC_ATTEN_DB_6] - 100) + atten = ADC_ATTEN_DB_6; + else if (mv > thresholds[ADC_ATTEN_DB_6] - 50) + atten = ADC_ATTEN_DB_11; + else return; + + adc1_set_attenuation(chan, atten); +} + +// ------------------------ +// PWM +// ------------------------ + +int8_t channel_for_pin(const uint8_t pin) { + for (int i = 0; i <= CHANNEL_MAX_NUM; i++) + if (chan_pin[i] == pin) return i; + return -1; +} + +// get PWM channel for pin - if none then attach a new one +// return -1 if fail or invalid pin#, channel # (0-15) if success +int8_t get_pwm_channel(const pin_t pin, const uint32_t freq, const uint16_t res) { + if (!WITHIN(pin, 1, MAX_PWM_IOPIN)) return -1; // Not a hardware PWM pin! + int8_t cid = channel_for_pin(pin); + if (cid >= 0) return cid; + + // Find an empty adjacent channel (same timer & freq/res) + for (int i = 0; i <= CHANNEL_MAX_NUM; i++) { + if (chan_pin[i] == 0) { + if (chan_pin[i ^ 0x1] != 0) { + if (pwmInfo[i / 2].freq == freq && pwmInfo[i / 2].res == res) { + chan_pin[i] = pin; // Allocate PWM to this channel + ledcAttachPin(pin, i); + return i; + } + } + else if (cid == -1) // Pair of empty channels? + cid = i & 0xFE; // Save lower channel number + } + } + // not attached, is an empty timer slot avail? + if (cid >= 0) { + chan_pin[cid] = pin; + pwmInfo[cid / 2].freq = freq; + pwmInfo[cid / 2].res = res; + ledcSetup(cid, freq, res); + ledcAttachPin(pin, cid); + } + return cid; // -1 if no channel avail +} + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=_BV(PWM_RESOLUTION)-1*/, const bool invert/*=false*/) { + #if ENABLED(I2S_STEPPER_STREAM) + if (pin > 127) { + const uint8_t pinlo = pin & 0x7F; + pwm_pin_t &pindata = pwm_pin_data[pinlo]; + const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, pindata.pwm_cycle_ticks); + if (duty == 0 || duty == pindata.pwm_cycle_ticks) { // max or min (i.e., on/off) + pindata.pwm_duty_ticks = 0; // turn off PWM for this pin + duty ? SBI32(i2s_port_data, pinlo) : CBI32(i2s_port_data, pinlo); // set pin level + } + else + pindata.pwm_duty_ticks = duty; // PWM duty count = # of 4µs ticks per full PWM cycle + } + else + #endif + { + const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION); + if (cid >= 0) { + const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1); + ledcWrite(cid, duty); + } + } +} + +int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) { + #if ENABLED(I2S_STEPPER_STREAM) + if (pin > 127) { + pwm_pin_data[pin & 0x7F].pwm_cycle_ticks = 1000000UL / f_desired / 4; // # of 4µs ticks per full PWM cycle + return 0; + } + else + #endif + { + const int8_t cid = channel_for_pin(pin); + if (cid >= 0) { + if (f_desired == ledcReadFreq(cid)) return cid; // no freq change + ledcDetachPin(chan_pin[cid]); + chan_pin[cid] = 0; // remove old freq channel + } + return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one + } +} + +// use hardware PWM if avail, if not then ISR +void analogWrite(const pin_t pin, const uint16_t value, const uint32_t freq/*=PWM_FREQUENCY*/, const uint16_t res/*=8*/) { // always 8 bit resolution! + // Use ledc hardware for internal pins + const int8_t cid = get_pwm_channel(pin, freq, res); + if (cid >= 0) { + ledcWrite(cid, value); // set duty value + return; + } + + // not a hardware PWM pin OR no PWM channels available + int idx = -1; + + // Search Pin + for (int i = 0; i < numPWMUsed; ++i) + if (pwmState[i].pin == pin) { idx = i; break; } + + // not found ? + if (idx < 0) { + // No slots remaining + if (numPWMUsed >= MAX_PWM_PINS) return; + + // Take new slot for pin + idx = numPWMUsed; + pwmState[idx].pin = pin; + // Start timer on first use + if (idx == 0) HAL_timer_start(MF_TIMER_PWM, PWM_TIMER_FREQUENCY); + + ++numPWMUsed; + } + + // Use 7bit internal value - add 1 to have 100% high at 255 + pwmState[idx].value = (value + 1) / 2; +} + +// Handle PWM timer interrupt +HAL_PWM_TIMER_ISR() { + HAL_timer_isr_prologue(MF_TIMER_PWM); + + static uint8_t count = 0; + + for (int i = 0; i < numPWMUsed; ++i) { + if (count == 0) // Start of interval + digitalWrite(pwmState[i].pin, pwmState[i].value ? HIGH : LOW); + else if (pwmState[i].value == count) // End of duration + digitalWrite(pwmState[i].pin, LOW); + } + + // 128 for 7 Bit resolution + count = (count + 1) & 0x7F; + + HAL_timer_isr_epilogue(MF_TIMER_PWM); +} + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/HAL.h b/src/HAL/ESP32/HAL.h new file mode 100644 index 0000000..ddfedf9 --- /dev/null +++ b/src/HAL/ESP32/HAL.h @@ -0,0 +1,246 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Espressif ESP32 WiFi + */ + +#define CPU_32_BIT + +#include + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" + +#include "fastio.h" +#include "i2s.h" + +#if ENABLED(WIFISUPPORT) + #include "WebSocketSerial.h" +#endif + +#if ENABLED(ESP3D_WIFISUPPORT) + #include "esp3dlib.h" +#endif + +#include "FlushableHardwareSerial.h" + +// ------------------------ +// Defines +// ------------------------ + +#define MYSERIAL1 flushableSerial + +#if EITHER(WIFISUPPORT, ESP3D_WIFISUPPORT) + #if ENABLED(ESP3D_WIFISUPPORT) + typedef ForwardSerial1Class< decltype(Serial2Socket) > DefaultSerial1; + extern DefaultSerial1 MSerial0; + #define MYSERIAL2 MSerial0 + #else + #define MYSERIAL2 webSocketSerial + #endif +#endif + +#define CRITICAL_SECTION_START() portENTER_CRITICAL(&hal.spinlock) +#define CRITICAL_SECTION_END() portEXIT_CRITICAL(&hal.spinlock) + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment +#define PWM_FREQUENCY 1000u // Default PWM frequency when set_pwm_duty() is called without set_pwm_frequency() +#define PWM_RESOLUTION 10u // Default PWM bit resolution +#define CHANNEL_MAX_NUM 15u // max PWM channel # to allocate (7 to only use low speed, 15 to use low & high) +#define MAX_PWM_IOPIN 33u // hardware pwm pins < 34 +#ifndef MAX_EXPANDER_BITS + #define MAX_EXPANDER_BITS 32 // I2S expander bit width (max 32) +#endif + +// ------------------------ +// Types +// ------------------------ + +typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs. +typedef int16_t pin_t; + +typedef struct pwm_pin { + uint32_t pwm_cycle_ticks = 1000000UL / (PWM_FREQUENCY) / 4; // # ticks per pwm cycle + uint32_t pwm_tick_count = 0; // current tick count + uint32_t pwm_duty_ticks = 0; // # of ticks for current duty cycle +} pwm_pin_t; + +class Servo; +typedef Servo hal_servo_t; + +// ------------------------ +// Public functions +// ------------------------ + +// +// Tone +// +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); +void noTone(const pin_t _pin); +int8_t get_pwm_channel(const pin_t pin, const uint32_t freq, const uint16_t res); +void analogWrite(const pin_t pin, const uint16_t value, const uint32_t freq=PWM_FREQUENCY, const uint16_t res=8); + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +#if ENABLED(USE_ESP32_EXIO) + void Write_EXIO(uint8_t IO, uint8_t v); +#endif + +// +// Delay in cycles (used by DELAY_NS / DELAY_US) +// +FORCE_INLINE static void DELAY_CYCLES(uint32_t x) { + unsigned long start, ccount, stop; + + /** + * It's important to care for race conditions (and overflows) here. + * Race condition example: If `stop` calculates to being close to the upper boundary of + * `uint32_t` and if at the same time a longer loop interruption kicks in (e.g. due to other + * FreeRTOS tasks or interrupts), `ccount` might overflow (and therefore be below `stop` again) + * without the loop ever being able to notice that `ccount` had already been above `stop` once + * (and that therefore the number of cycles to delay has already passed). + * As DELAY_CYCLES (through DELAY_NS / DELAY_US) is used by software SPI bit banging to drive + * LCDs and therefore might be called very, very often, this seemingly improbable situation did + * actually happen in reality. It resulted in apparently random print pauses of ~17.9 seconds + * (0x100000000 / 240 MHz) or multiples thereof, essentially ruining the current print by causing + * large blobs of filament. + */ + + __asm__ __volatile__ ( "rsr %0, ccount" : "=a" (start) ); + stop = start + x; + ccount = start; + + if (stop >= start) { + // no overflow, so only loop while in between start and stop: + // 0x00000000 -----------------start****stop-- 0xFFFFFFFF + while (ccount >= start && ccount < stop) { + __asm__ __volatile__ ( "rsr %0, ccount" : "=a" (ccount) ); + } + } + else { + // stop did overflow, so only loop while outside of stop and start: + // 0x00000000 **stop-------------------start** 0xFFFFFFFF + while (ccount >= start || ccount < stop) { + __asm__ __volatile__ ( "rsr %0, ccount" : "=a" (ccount) ); + } + } + +} + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +void _delay_ms(const int ms); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init() {} // Called early in setup() + static void init_board(); // Called less early in setup() + static void reboot(); // Restart the firmware + + // Interrupts + static portMUX_TYPE spinlock; + static bool isr_state() { return spinlock.owner == portMUX_FREE_VAL; } + static void isr_on() { if (spinlock.owner != portMUX_FREE_VAL) portEXIT_CRITICAL(&spinlock); } + static void isr_off() { portENTER_CRITICAL(&spinlock); } + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory(); + + static pwm_pin_t pwm_pin_data[MAX_EXPANDER_BITS]; + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) {} + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * If not already allocated, allocate a hardware PWM channel + * to the pin and set the duty cycle.. + * Optionally invert the duty cycle [default = false] + * Optionally change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Allocate and set the frequency of a hardware PWM pin + * Returns -1 if no pin available. + */ + static int8_t set_pwm_frequency(const pin_t pin, const uint32_t f_desired); + +}; diff --git a/src/HAL/ESP32/HAL_SPI.cpp b/src/HAL/ESP32/HAL_SPI.cpp new file mode 100644 index 0000000..868ab1b --- /dev/null +++ b/src/HAL/ESP32/HAL_SPI.cpp @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" + +#include "../shared/HAL_SPI.h" + +#include +#include + +// ------------------------ +// Public Variables +// ------------------------ + +static SPISettings spiConfig; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(SOFTWARE_SPI) + + // ------------------------ + // Software SPI + // ------------------------ + #error "Software SPI not supported for ESP32. Use Hardware SPI." + +#else + +// ------------------------ +// Hardware SPI +// ------------------------ + +void spiBegin() { + #if ENABLED(SDSUPPORT) && PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif +} + +void spiInit(uint8_t spiRate) { + uint32_t clock; + + switch (spiRate) { + case SPI_FULL_SPEED: clock = 16000000; break; + case SPI_HALF_SPEED: clock = 8000000; break; + case SPI_QUARTER_SPEED: clock = 4000000; break; + case SPI_EIGHTH_SPEED: clock = 2000000; break; + case SPI_SIXTEENTH_SPEED: clock = 1000000; break; + case SPI_SPEED_5: clock = 500000; break; + case SPI_SPEED_6: clock = 250000; break; + default: clock = 1000000; // Default from the SPI library + } + + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + SPI.begin(); +} + +uint8_t spiRec() { + SPI.beginTransaction(spiConfig); + uint8_t returnByte = SPI.transfer(0xFF); + SPI.endTransaction(); + return returnByte; +} + +void spiRead(uint8_t *buf, uint16_t nbyte) { + SPI.beginTransaction(spiConfig); + SPI.transferBytes(0, buf, nbyte); + SPI.endTransaction(); +} + +void spiSend(uint8_t b) { + SPI.beginTransaction(spiConfig); + SPI.transfer(b); + SPI.endTransaction(); +} + +void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI.beginTransaction(spiConfig); + SPI.transfer(token); + SPI.writeBytes(const_cast(buf), 512); + SPI.endTransaction(); +} + +void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + spiConfig = SPISettings(spiClock, bitOrder, dataMode); + + SPI.beginTransaction(spiConfig); +} + +#endif // !SOFTWARE_SPI + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/MarlinSPI.h b/src/HAL/ESP32/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/ESP32/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/ESP32/Servo.cpp b/src/HAL/ESP32/Servo.cpp new file mode 100644 index 0000000..ca3950d --- /dev/null +++ b/src/HAL/ESP32/Servo.cpp @@ -0,0 +1,67 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "Servo.h" + +// Adjacent channels (0/1, 2/3 etc.) share the same timer and therefore the same frequency and resolution settings on ESP32, +// so we only allocate servo channels up high to avoid side effects with regards to analogWrite (fans, leds, laser pwm etc.) +int Servo::channel_next_free = 12; + +Servo::Servo() {} + +int8_t Servo::attach(const int inPin) { + if (inPin > 0) pin = inPin; + channel = get_pwm_channel(pin, 50u, 16u); + return channel; // -1 if no PWM avail. +} + +// leave channel connected to servo - set duty to zero +void Servo::detach() { + if (channel >= 0) ledcWrite(channel, 0); +} + +int Servo::read() { return degrees; } + +void Servo::write(int inDegrees) { + degrees = constrain(inDegrees, MIN_ANGLE, MAX_ANGLE); + int us = map(degrees, MIN_ANGLE, MAX_ANGLE, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); + int duty = map(us, 0, TAU_USEC, 0, MAX_COMPARE); + if (channel >= 0) ledcWrite(channel, duty); // don't save duty for servos! +} + +void Servo::move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + if (attach(0) >= 0) { + write(value); + safe_delay(servo_delay[channel]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} +#endif // HAS_SERVOS + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/Servo.h b/src/HAL/ESP32/Servo.h new file mode 100644 index 0000000..1dbb416 --- /dev/null +++ b/src/HAL/ESP32/Servo.h @@ -0,0 +1,48 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +class Servo { + static const int MIN_ANGLE = 0, + MAX_ANGLE = 180, + MIN_PULSE_WIDTH = 544, // Shortest pulse sent to a servo + MAX_PULSE_WIDTH = 2400, // Longest pulse sent to a servo + TAU_MSEC = 20, + TAU_USEC = (TAU_MSEC * 1000), + MAX_COMPARE = _BV(16) - 1; // 65535 + +public: + Servo(); + int8_t attach(const int pin); // attach the given pin to the next free channel, set pinMode, return channel number (-1 on fail) + void detach(); + void write(int degrees); // set angle + void move(const int degrees); // attach the servo, then move to value + int read(); // returns current pulse width as an angle between 0 and 180 degrees + +private: + static int channel_next_free; + int channel; + int pin; + int degrees; +}; diff --git a/src/HAL/ESP32/Tone.cpp b/src/HAL/ESP32/Tone.cpp new file mode 100644 index 0000000..839c612 --- /dev/null +++ b/src/HAL/ESP32/Tone.cpp @@ -0,0 +1,59 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * Copypaste of SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Description: Tone function for ESP32 + * Derived from https://forum.arduino.cc/index.php?topic=136500.msg2903012#msg2903012 + */ + +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +static pin_t tone_pin; +volatile static int32_t toggles; + +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration/*=0*/) { + tone_pin = _pin; + toggles = 2 * frequency * duration / 1000; + HAL_timer_start(MF_TIMER_TONE, 2 * frequency); +} + +void noTone(const pin_t _pin) { + HAL_timer_disable_interrupt(MF_TIMER_TONE); + WRITE(_pin, LOW); +} + +HAL_TONE_TIMER_ISR() { + HAL_timer_isr_prologue(MF_TIMER_TONE); + + if (toggles) { + toggles--; + TOGGLE(tone_pin); + } + else noTone(tone_pin); // turn off interrupt +} + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/WebSocketSerial.cpp b/src/HAL/ESP32/WebSocketSerial.cpp new file mode 100644 index 0000000..eb5b9d6 --- /dev/null +++ b/src/HAL/ESP32/WebSocketSerial.cpp @@ -0,0 +1,148 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(WIFISUPPORT) + +#include "WebSocketSerial.h" +#include "wifi.h" +#include + +MSerialWebSocketT webSocketSerial(false); +AsyncWebSocket ws("/ws"); // TODO Move inside the class. + +// RingBuffer impl + +#define NEXT_INDEX(I, SIZE) ((I + 1) & (ring_buffer_pos_t)(SIZE - 1)) + +RingBuffer::RingBuffer(ring_buffer_pos_t size) + : data(new uint8_t[size]), + size(size), + read_index(0), + write_index(0) +{} + +RingBuffer::~RingBuffer() { delete[] data; } + +ring_buffer_pos_t RingBuffer::write(const uint8_t c) { + const ring_buffer_pos_t n = NEXT_INDEX(write_index, size); + + if (n != read_index) { + this->data[write_index] = c; + write_index = n; + return 1; + } + + // TODO: buffer is full, handle? + return 0; +} + +ring_buffer_pos_t RingBuffer::write(const uint8_t *buffer, ring_buffer_pos_t size) { + ring_buffer_pos_t written = 0; + for (ring_buffer_pos_t i = 0; i < size; i++) { + written += write(buffer[i]); + } + return written; +} + +int RingBuffer::available() { + return (size - read_index + write_index) & (size - 1); +} + +int RingBuffer::peek() { + return available() ? data[read_index] : -1; +} + +int RingBuffer::read() { + if (available()) { + const int ret = data[read_index]; + read_index = NEXT_INDEX(read_index, size); + return ret; + } + return -1; +} + +ring_buffer_pos_t RingBuffer::read(uint8_t *buffer) { + ring_buffer_pos_t len = available(); + + for (ring_buffer_pos_t i = 0; read_index != write_index; i++) { + buffer[i] = data[read_index]; + read_index = NEXT_INDEX(read_index, size); + } + + return len; +} + +void RingBuffer::flush() { read_index = write_index; } + +// WebSocketSerial impl +WebSocketSerial::WebSocketSerial() + : rx_buffer(RingBuffer(RX_BUFFER_SIZE)), + tx_buffer(RingBuffer(TX_BUFFER_SIZE)) +{} + +void WebSocketSerial::begin(const long baud_setting) { + ws.onEvent([this](AsyncWebSocket *server, AsyncWebSocketClient *client, AwsEventType type, void *arg, uint8_t *data, size_t len) { + switch (type) { + case WS_EVT_CONNECT: client->ping(); break; // client connected + case WS_EVT_DISCONNECT: // client disconnected + case WS_EVT_ERROR: // error was received from the other end + case WS_EVT_PONG: break; // pong message was received (in response to a ping request maybe) + case WS_EVT_DATA: { // data packet + AwsFrameInfo * info = (AwsFrameInfo*)arg; + if (info->opcode == WS_TEXT || info->message_opcode == WS_TEXT) + this->rx_buffer.write(data, len); + } + } + }); + server.addHandler(&ws); +} + +void WebSocketSerial::end() { } +int WebSocketSerial::peek() { return rx_buffer.peek(); } +int WebSocketSerial::read() { return rx_buffer.read(); } +int WebSocketSerial::available() { return rx_buffer.available(); } +void WebSocketSerial::flush() { rx_buffer.flush(); } + +size_t WebSocketSerial::write(const uint8_t c) { + size_t ret = tx_buffer.write(c); + + if (ret && c == '\n') { + uint8_t tmp[TX_BUFFER_SIZE]; + ring_buffer_pos_t size = tx_buffer.read(tmp); + ws.textAll(tmp, size); + } + + return ret; +} + +size_t WebSocketSerial::write(const uint8_t *buffer, size_t size) { + size_t written = 0; + for (size_t i = 0; i < size; i++) + written += write(buffer[i]); + return written; +} + +#endif // WIFISUPPORT +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/WebSocketSerial.h b/src/HAL/ESP32/WebSocketSerial.h new file mode 100644 index 0000000..6b3e419 --- /dev/null +++ b/src/HAL/ESP32/WebSocketSerial.h @@ -0,0 +1,85 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" +#include "../../core/serial_hook.h" + +#include + +#ifndef TX_BUFFER_SIZE + #define TX_BUFFER_SIZE 32 +#endif +#if ENABLED(WIFISUPPORT) + #ifndef RX_BUFFER_SIZE + #define RX_BUFFER_SIZE 128 + #endif + #if TX_BUFFER_SIZE <= 0 + #error "TX_BUFFER_SIZE is required for the WebSocket." + #endif +#endif + +typedef uint16_t ring_buffer_pos_t; + +class RingBuffer { + uint8_t *data; + ring_buffer_pos_t size, read_index, write_index; + +public: + RingBuffer(ring_buffer_pos_t size); + ~RingBuffer(); + + int available(); + int peek(); + int read(); + ring_buffer_pos_t read(uint8_t *buffer); + void flush(); + ring_buffer_pos_t write(const uint8_t c); + ring_buffer_pos_t write(const uint8_t *buffer, ring_buffer_pos_t size); +}; + +class WebSocketSerial: public Stream { + RingBuffer rx_buffer; + RingBuffer tx_buffer; + +public: + WebSocketSerial(); + void begin(const long); + void end(); + int available(); + int peek(); + int read(); + void flush(); + size_t write(const uint8_t c); + size_t write(const uint8_t *buffer, size_t size); + + #if ENABLED(SERIAL_STATS_DROPPED_RX) + FORCE_INLINE uint32_t dropped() { return 0; } + #endif + + #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + FORCE_INLINE int rxMaxEnqueued() { return 0; } + #endif +}; + +typedef Serial1Class MSerialWebSocketT; +extern MSerialWebSocketT webSocketSerial; diff --git a/src/HAL/ESP32/eeprom.cpp b/src/HAL/ESP32/eeprom.cpp new file mode 100644 index 0000000..cb5f881 --- /dev/null +++ b/src/HAL/ESP32/eeprom.cpp @@ -0,0 +1,57 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(EEPROM_SETTINGS) + +#include "../shared/eeprom_api.h" +#include + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { return EEPROM.begin(MARLIN_EEPROM_SIZE); } +bool PersistentStore::access_finish() { EEPROM.end(); return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + for (size_t i = 0; i < size; i++) { + EEPROM.write(pos++, value[i]); + crc16(crc, &value[i], 1); + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + for (size_t i = 0; i < size; i++) { + uint8_t c = EEPROM.read(pos++); + if (writing) value[i] = c; + crc16(crc, &c, 1); + } + return false; +} + +#endif // EEPROM_SETTINGS +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/endstop_interrupts.h b/src/HAL/ESP32/endstop_interrupts.h new file mode 100644 index 0000000..4725df9 --- /dev/null +++ b/src/HAL/ESP32/endstop_interrupts.h @@ -0,0 +1,68 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the stepper-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void ICACHE_RAM_ATTR endstop_ISR() { endstops.update(); } + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/ESP32/esp32.csv b/src/HAL/ESP32/esp32.csv new file mode 100644 index 0000000..6360bbc --- /dev/null +++ b/src/HAL/ESP32/esp32.csv @@ -0,0 +1,6 @@ +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x5000, +otadata, data, ota, 0xe000, 0x2000, +app0, app, ota_0, 0x10000, 0x180000, +app1, app, ota_1, 0x190000, 0x180000, +spiffs, data, spiffs, 0x310000, 0xF0000, diff --git a/src/HAL/ESP32/fastio.h b/src/HAL/ESP32/fastio.h new file mode 100644 index 0000000..c8e3f7e --- /dev/null +++ b/src/HAL/ESP32/fastio.h @@ -0,0 +1,93 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "i2s.h" + +/** + * Utility functions + */ + +// I2S expander pin mapping. +#define IS_I2S_EXPANDER_PIN(IO) TEST(IO, 7) +#define I2S_EXPANDER_PIN_INDEX(IO) (IO & 0x7F) + +// Set pin as input +#define _SET_INPUT(IO) pinMode(IO, INPUT) + +// Set pin as output +#define _SET_OUTPUT(IO) pinMode(IO, OUTPUT) + +// Set pin as input with pullup mode +#define _PULLUP(IO, v) pinMode(IO, v ? INPUT_PULLUP : INPUT) + +#if ENABLED(USE_ESP32_EXIO) + // Read a pin wrapper + #define READ(IO) digitalRead(IO) + // Write to a pin wrapper + #define WRITE(IO, v) (IO >= 100 ? Write_EXIO(IO, v) : digitalWrite(IO, v)) +#else + // Read a pin wrapper + #define READ(IO) (IS_I2S_EXPANDER_PIN(IO) ? i2s_state(I2S_EXPANDER_PIN_INDEX(IO)) : digitalRead(IO)) + // Write to a pin wrapper + #define WRITE(IO, v) (IS_I2S_EXPANDER_PIN(IO) ? i2s_write(I2S_EXPANDER_PIN_INDEX(IO), v) : digitalWrite(IO, v)) +#endif + +// Set pin as input wrapper (0x80 | (v << 5) | (IO - 100)) +#define SET_INPUT(IO) _SET_INPUT(IO) + +// Set pin as input with pullup wrapper +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) + +// Set pin as input with pulldown (substitution) +#define SET_INPUT_PULLDOWN SET_INPUT + +// Set pin as output wrapper +#define SET_OUTPUT(IO) do{ _SET_OUTPUT(IO); }while(0) + +// Set pin as PWM +#define SET_PWM SET_OUTPUT + +// Set pin as output and init +#define OUT_WRITE(IO,V) do{ _SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +// PWM outputs +#define PWM_PIN(P) (P < 34 || P > 127) // NOTE Pins >= 34 are input only on ESP32, so they can't be used for output. + +// Toggle pin value +#define TOGGLE(IO) WRITE(IO, !READ(IO)) + +// +// Ports and functions +// + +// UART +#define RXD 3 +#define TXD 1 + +// TWI (I2C) +#define SCL 5 +#define SDA 4 diff --git a/src/HAL/ESP32/i2s.cpp b/src/HAL/ESP32/i2s.cpp new file mode 100644 index 0000000..cf337ee --- /dev/null +++ b/src/HAL/ESP32/i2s.cpp @@ -0,0 +1,364 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if DISABLED(USE_ESP32_EXIO) + +#include "i2s.h" + +#include "../shared/Marduino.h" +#include +#include +#include +#include +#include "../../module/stepper.h" + +#define DMA_BUF_COUNT 8 // number of DMA buffers to store data +#define DMA_BUF_LEN 4092 // maximum size in bytes +#define I2S_SAMPLE_SIZE 4 // 4 bytes, 32 bits per sample +#define DMA_SAMPLE_COUNT DMA_BUF_LEN / I2S_SAMPLE_SIZE // number of samples per buffer + +typedef enum { + I2S_NUM_0 = 0x0, /*!< I2S 0*/ + I2S_NUM_1 = 0x1, /*!< I2S 1*/ + I2S_NUM_MAX, +} i2s_port_t; + +typedef struct { + uint32_t **buffers; + uint32_t *current; + uint32_t rw_pos; + lldesc_t **desc; + xQueueHandle queue; +} i2s_dma_t; + +static portMUX_TYPE i2s_spinlock[I2S_NUM_MAX] = {portMUX_INITIALIZER_UNLOCKED, portMUX_INITIALIZER_UNLOCKED}; +static i2s_dev_t* I2S[I2S_NUM_MAX] = {&I2S0, &I2S1}; +static i2s_dma_t dma; + +// output value +uint32_t i2s_port_data = 0; + +#define I2S_ENTER_CRITICAL() portENTER_CRITICAL(&i2s_spinlock[i2s_num]) +#define I2S_EXIT_CRITICAL() portEXIT_CRITICAL(&i2s_spinlock[i2s_num]) + +static inline void gpio_matrix_out_check(uint32_t gpio, uint32_t signal_idx, bool out_inv, bool oen_inv) { + PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[gpio], PIN_FUNC_GPIO); + gpio_set_direction((gpio_num_t)gpio, (gpio_mode_t)GPIO_MODE_DEF_OUTPUT); + gpio_matrix_out(gpio, signal_idx, out_inv, oen_inv); +} + +static esp_err_t i2s_reset_fifo(i2s_port_t i2s_num) { + I2S_ENTER_CRITICAL(); + I2S[i2s_num]->conf.rx_fifo_reset = 1; + I2S[i2s_num]->conf.rx_fifo_reset = 0; + I2S[i2s_num]->conf.tx_fifo_reset = 1; + I2S[i2s_num]->conf.tx_fifo_reset = 0; + I2S_EXIT_CRITICAL(); + + return ESP_OK; +} + +esp_err_t i2s_start(i2s_port_t i2s_num) { + //start DMA link + I2S_ENTER_CRITICAL(); + i2s_reset_fifo(i2s_num); + + //reset dma + I2S[i2s_num]->lc_conf.in_rst = 1; + I2S[i2s_num]->lc_conf.in_rst = 0; + I2S[i2s_num]->lc_conf.out_rst = 1; + I2S[i2s_num]->lc_conf.out_rst = 0; + + I2S[i2s_num]->conf.tx_reset = 1; + I2S[i2s_num]->conf.tx_reset = 0; + I2S[i2s_num]->conf.rx_reset = 1; + I2S[i2s_num]->conf.rx_reset = 0; + + I2S[i2s_num]->int_clr.val = 0xFFFFFFFF; + I2S[i2s_num]->out_link.start = 1; + I2S[i2s_num]->conf.tx_start = 1; + I2S_EXIT_CRITICAL(); + + return ESP_OK; +} + +esp_err_t i2s_stop(i2s_port_t i2s_num) { + I2S_ENTER_CRITICAL(); + I2S[i2s_num]->out_link.stop = 1; + I2S[i2s_num]->conf.tx_start = 0; + + I2S[i2s_num]->int_clr.val = I2S[i2s_num]->int_st.val; //clear pending interrupt + I2S_EXIT_CRITICAL(); + + return ESP_OK; +} + +static void IRAM_ATTR i2s_intr_handler_default(void *arg) { + int dummy; + lldesc_t *finish_desc; + portBASE_TYPE high_priority_task_awoken = pdFALSE; + + if (I2S0.int_st.out_eof) { + // Get the descriptor of the last item in the linkedlist + finish_desc = (lldesc_t*) I2S0.out_eof_des_addr; + + // If the queue is full it's because we have an underflow, + // more than buf_count isr without new data, remove the front buffer + if (xQueueIsQueueFullFromISR(dma.queue)) + xQueueReceiveFromISR(dma.queue, &dummy, &high_priority_task_awoken); + + xQueueSendFromISR(dma.queue, (void *)(&finish_desc->buf), &high_priority_task_awoken); + } + + if (high_priority_task_awoken == pdTRUE) portYIELD_FROM_ISR(); + + // clear interrupt + I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt +} + +void stepperTask(void *parameter) { + uint32_t remaining = 0; + + while (1) { + xQueueReceive(dma.queue, &dma.current, portMAX_DELAY); + dma.rw_pos = 0; + + while (dma.rw_pos < DMA_SAMPLE_COUNT) { + // Fill with the port data post pulse_phase until the next step + if (remaining) { + i2s_push_sample(); + remaining--; + } + else { + Stepper::pulse_phase_isr(); + remaining = Stepper::block_phase_isr(); + } + } + } +} + +int i2s_init() { + periph_module_enable(PERIPH_I2S0_MODULE); + + /** + * Each i2s transfer will take + * fpll = PLL_D2_CLK -- clka_en = 0 + * + * fi2s = fpll / N + b/a -- N = clkm_div_num + * fi2s = 160MHz / 2 + * fi2s = 80MHz + * + * fbclk = fi2s / M -- M = tx_bck_div_num + * fbclk = 80MHz / 2 + * fbclk = 40MHz + * + * fwclk = fbclk / 32 + * + * for fwclk = 250kHz (4µS pulse time) + * N = 10 + * M = 20 + */ + + // Allocate the array of pointers to the buffers + dma.buffers = (uint32_t **)malloc(sizeof(uint32_t*) * DMA_BUF_COUNT); + if (!dma.buffers) return -1; + + // Allocate each buffer that can be used by the DMA controller + for (int buf_idx = 0; buf_idx < DMA_BUF_COUNT; buf_idx++) { + dma.buffers[buf_idx] = (uint32_t*) heap_caps_calloc(1, DMA_BUF_LEN, MALLOC_CAP_DMA); + if (dma.buffers[buf_idx] == nullptr) return -1; + } + + // Allocate the array of DMA descriptors + dma.desc = (lldesc_t**) malloc(sizeof(lldesc_t*) * DMA_BUF_COUNT); + if (!dma.desc) return -1; + + // Allocate each DMA descriptor that will be used by the DMA controller + for (int buf_idx = 0; buf_idx < DMA_BUF_COUNT; buf_idx++) { + dma.desc[buf_idx] = (lldesc_t*) heap_caps_malloc(sizeof(lldesc_t), MALLOC_CAP_DMA); + if (dma.desc[buf_idx] == nullptr) return -1; + } + + // Initialize + for (int buf_idx = 0; buf_idx < DMA_BUF_COUNT; buf_idx++) { + dma.desc[buf_idx]->owner = 1; + dma.desc[buf_idx]->eof = 1; // set to 1 will trigger the interrupt + dma.desc[buf_idx]->sosf = 0; + dma.desc[buf_idx]->length = DMA_BUF_LEN; + dma.desc[buf_idx]->size = DMA_BUF_LEN; + dma.desc[buf_idx]->buf = (uint8_t *) dma.buffers[buf_idx]; + dma.desc[buf_idx]->offset = 0; + dma.desc[buf_idx]->empty = (uint32_t)((buf_idx < (DMA_BUF_COUNT - 1)) ? (dma.desc[buf_idx + 1]) : dma.desc[0]); + } + + dma.queue = xQueueCreate(DMA_BUF_COUNT, sizeof(uint32_t *)); + + // Set the first DMA descriptor + I2S0.out_link.addr = (uint32_t)dma.desc[0]; + + // stop i2s + i2s_stop(I2S_NUM_0); + + // configure I2S data port interface. + i2s_reset_fifo(I2S_NUM_0); + + //reset i2s + I2S0.conf.tx_reset = 1; + I2S0.conf.tx_reset = 0; + I2S0.conf.rx_reset = 1; + I2S0.conf.rx_reset = 0; + + //reset dma + I2S0.lc_conf.in_rst = 1; + I2S0.lc_conf.in_rst = 0; + I2S0.lc_conf.out_rst = 1; + I2S0.lc_conf.out_rst = 0; + + //Enable and configure DMA + I2S0.lc_conf.check_owner = 0; + I2S0.lc_conf.out_loop_test = 0; + I2S0.lc_conf.out_auto_wrback = 0; + I2S0.lc_conf.out_data_burst_en = 0; + I2S0.lc_conf.outdscr_burst_en = 0; + I2S0.lc_conf.out_no_restart_clr = 0; + I2S0.lc_conf.indscr_burst_en = 0; + I2S0.lc_conf.out_eof_mode = 1; + + I2S0.conf2.lcd_en = 0; + I2S0.conf2.camera_en = 0; + I2S0.pdm_conf.pcm2pdm_conv_en = 0; + I2S0.pdm_conf.pdm2pcm_conv_en = 0; + + I2S0.fifo_conf.dscr_en = 0; + + I2S0.conf_chan.tx_chan_mod = TERN(I2S_STEPPER_SPLIT_STREAM, 4, 0); + I2S0.fifo_conf.tx_fifo_mod = 0; + I2S0.conf.tx_mono = 0; + + I2S0.conf_chan.rx_chan_mod = 0; + I2S0.fifo_conf.rx_fifo_mod = 0; + I2S0.conf.rx_mono = 0; + + I2S0.fifo_conf.dscr_en = 1; //connect dma to fifo + + I2S0.conf.tx_start = 0; + I2S0.conf.rx_start = 0; + + I2S0.conf.tx_msb_right = 1; + I2S0.conf.tx_right_first = 1; + + I2S0.conf.tx_slave_mod = 0; // Master + I2S0.fifo_conf.tx_fifo_mod_force_en = 1; + + I2S0.pdm_conf.rx_pdm_en = 0; + I2S0.pdm_conf.tx_pdm_en = 0; + + I2S0.conf.tx_short_sync = 0; + I2S0.conf.rx_short_sync = 0; + I2S0.conf.tx_msb_shift = 0; + I2S0.conf.rx_msb_shift = 0; + + // set clock + I2S0.clkm_conf.clka_en = 0; // Use PLL/2 as reference + I2S0.clkm_conf.clkm_div_num = 10; // minimum value of 2, reset value of 4, max 256 + I2S0.clkm_conf.clkm_div_a = 0; // 0 at reset, what about divide by 0? (not an issue) + I2S0.clkm_conf.clkm_div_b = 0; // 0 at reset + + // fbck = fi2s / tx_bck_div_num + I2S0.sample_rate_conf.tx_bck_div_num = 2; // minimum value of 2 defaults to 6 + + // Enable TX interrupts + I2S0.int_ena.out_eof = 1; + I2S0.int_ena.out_dscr_err = 0; + I2S0.int_ena.out_total_eof = 0; + I2S0.int_ena.out_done = 0; + + // Allocate and Enable the I2S interrupt + intr_handle_t i2s_isr_handle; + esp_intr_alloc(ETS_I2S0_INTR_SOURCE, 0, i2s_intr_handler_default, nullptr, &i2s_isr_handle); + esp_intr_enable(i2s_isr_handle); + + // Create the task that will feed the buffer + xTaskCreatePinnedToCore(stepperTask, "StepperTask", 10000, nullptr, 1, nullptr, CONFIG_ARDUINO_RUNNING_CORE); // run I2S stepper task on same core as rest of Marlin + + // Route the i2s pins to the appropriate GPIO + // If a pin is not defined, no need to configure + #if defined(I2S_DATA) && I2S_DATA >= 0 + gpio_matrix_out_check(I2S_DATA, I2S0O_DATA_OUT23_IDX, 0, 0); + #endif + #if defined(I2S_BCK) && I2S_BCK >= 0 + gpio_matrix_out_check(I2S_BCK, I2S0O_BCK_OUT_IDX, 0, 0); + #endif + #if defined(I2S_WS) && I2S_WS >= 0 + gpio_matrix_out_check(I2S_WS, I2S0O_WS_OUT_IDX, 0, 0); + #endif + + // Start the I2S peripheral + return i2s_start(I2S_NUM_0); +} + +void i2s_write(uint8_t pin, uint8_t val) { + #if ENABLED(I2S_STEPPER_SPLIT_STREAM) + if (pin >= 16) { + SET_BIT_TO(I2S0.conf_single_data, pin, val); + return; + } + #endif + SET_BIT_TO(i2s_port_data, pin, val); +} + +uint8_t i2s_state(uint8_t pin) { + #if ENABLED(I2S_STEPPER_SPLIT_STREAM) + if (pin >= 16) return TEST(I2S0.conf_single_data, pin); + #endif + return TEST(i2s_port_data, pin); +} + +void i2s_push_sample() { + // Every 4µs (when space in DMA buffer) toggle each expander PWM output using + // the current duty cycle/frequency so they sync with any steps (once + // through the DMA/FIFO buffers). PWM signal inversion handled by other functions + LOOP_L_N(p, MAX_EXPANDER_BITS) { + if (hal.pwm_pin_data[p].pwm_duty_ticks > 0) { // pin has active pwm? + if (hal.pwm_pin_data[p].pwm_tick_count == 0) { + if (TEST32(i2s_port_data, p)) { // hi->lo + CBI32(i2s_port_data, p); + hal.pwm_pin_data[p].pwm_tick_count = hal.pwm_pin_data[p].pwm_cycle_ticks - hal.pwm_pin_data[p].pwm_duty_ticks; + } + else { // lo->hi + SBI32(i2s_port_data, p); + hal.pwm_pin_data[p].pwm_tick_count = hal.pwm_pin_data[p].pwm_duty_ticks; + } + } + else + hal.pwm_pin_data[p].pwm_tick_count--; + } + } + + dma.current[dma.rw_pos++] = i2s_port_data; +} + +#endif // !USE_ESP32_EXIO +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/i2s.h b/src/HAL/ESP32/i2s.h new file mode 100644 index 0000000..573b983 --- /dev/null +++ b/src/HAL/ESP32/i2s.h @@ -0,0 +1,35 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +// current value of the outputs provided over i2s +extern uint32_t i2s_port_data; + +int i2s_init(); + +uint8_t i2s_state(uint8_t pin); + +void i2s_write(uint8_t pin, uint8_t val); + +void i2s_push_sample(); diff --git a/src/HAL/ESP32/inc/Conditionals_LCD.h b/src/HAL/ESP32/inc/Conditionals_LCD.h new file mode 100644 index 0000000..4da6001 --- /dev/null +++ b/src/HAL/ESP32/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/ESP32." +#endif diff --git a/src/HAL/ESP32/inc/Conditionals_adv.h b/src/HAL/ESP32/inc/Conditionals_adv.h new file mode 100644 index 0000000..3ca8068 --- /dev/null +++ b/src/HAL/ESP32/inc/Conditionals_adv.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// +// Board-specific options need to be defined before HAL.h +// +#if MB(MKS_TINYBEE) + #define MAX_EXPANDER_BITS 24 // TinyBee has 3 x HC595 +#endif diff --git a/src/HAL/ESP32/inc/Conditionals_post.h b/src/HAL/ESP32/inc/Conditionals_post.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/ESP32/inc/Conditionals_post.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/ESP32/inc/SanityCheck.h b/src/HAL/ESP32/inc/SanityCheck.h new file mode 100644 index 0000000..3ccb159 --- /dev/null +++ b/src/HAL/ESP32/inc/SanityCheck.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if ENABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is not yet implemented for ESP32. Disable EMERGENCY_PARSER to continue." +#endif + +#if (ENABLED(SPINDLE_LASER_USE_PWM) && SPINDLE_LASER_FREQUENCY > 78125) || (ENABLED(FAST_PWM_FAN_FREQUENCY) && FAST_PWM_FAN_FREQUENCY > 78125) + #error "SPINDLE_LASER_FREQUENCY and FAST_PWM_FREQUENCY maximum value is 78125Hz for ESP32." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on ESP32." +#endif + +#if BOTH(WIFISUPPORT, ESP3D_WIFISUPPORT) + #error "Only enable one WiFi option, either WIFISUPPORT or ESP3D_WIFISUPPORT." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on ESP32." +#endif + +#if MB(MKS_TINYBEE) && ENABLED(FAST_PWM_FAN) + #error "FAST_PWM_FAN is not available on TinyBee." +#endif + +#if USING_PULLDOWNS + #error "PULLDOWN pin mode is not available on ESP32 boards." +#endif + +#if BOTH(I2S_STEPPER_STREAM, LIN_ADVANCE) + #error "I2S stream is currently incompatible with LIN_ADVANCE." +#endif diff --git a/src/HAL/ESP32/ota.cpp b/src/HAL/ESP32/ota.cpp new file mode 100644 index 0000000..69a3e25 --- /dev/null +++ b/src/HAL/ESP32/ota.cpp @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(WIFISUPPORT, OTASUPPORT) + +#include +#include +#include +#include +#include + +void OTA_init() { + ArduinoOTA + .onStart([]() { + timer_pause(TIMER_GROUP_0, TIMER_0); + timer_pause(TIMER_GROUP_0, TIMER_1); + + // U_FLASH or U_SPIFFS + String type = (ArduinoOTA.getCommand() == U_FLASH) ? "sketch" : "filesystem"; + + // NOTE: if updating SPIFFS this would be the place to unmount SPIFFS using SPIFFS.end() + Serial.println("Start updating " + type); + }) + .onEnd([]() { + Serial.println("\nEnd"); + }) + .onProgress([](unsigned int progress, unsigned int total) { + Serial.printf("Progress: %u%%\r", (progress / (total / 100))); + }) + .onError([](ota_error_t error) { + Serial.printf("Error[%u]: ", error); + char *str; + switch (error) { + case OTA_AUTH_ERROR: str = "Auth Failed"; break; + case OTA_BEGIN_ERROR: str = "Begin Failed"; break; + case OTA_CONNECT_ERROR: str = "Connect Failed"; break; + case OTA_RECEIVE_ERROR: str = "Receive Failed"; break; + case OTA_END_ERROR: str = "End Failed"; break; + } + Serial.println(str); + }); + + ArduinoOTA.begin(); +} + +void OTA_handle() { + ArduinoOTA.handle(); +} + +#endif // WIFISUPPORT && OTASUPPORT +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/ota.h b/src/HAL/ESP32/ota.h new file mode 100644 index 0000000..546ace8 --- /dev/null +++ b/src/HAL/ESP32/ota.h @@ -0,0 +1,23 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void OTA_init(); +void OTA_handle(); diff --git a/src/HAL/ESP32/servotimers.h b/src/HAL/ESP32/servotimers.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/ESP32/servotimers.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/ESP32/spi_pins.h b/src/HAL/ESP32/spi_pins.h new file mode 100644 index 0000000..58881f0 --- /dev/null +++ b/src/HAL/ESP32/spi_pins.h @@ -0,0 +1,27 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define SD_SS_PIN SDSS +#define SD_SCK_PIN 18 +#define SD_MISO_PIN 19 +#define SD_MOSI_PIN 23 diff --git a/src/HAL/ESP32/spiffs.cpp b/src/HAL/ESP32/spiffs.cpp new file mode 100644 index 0000000..a0e713b --- /dev/null +++ b/src/HAL/ESP32/spiffs.cpp @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(WIFISUPPORT, WEBSUPPORT) + +#include "../../core/serial.h" + +#include +#include + +bool spiffs_initialized; + +void spiffs_init() { + if (SPIFFS.begin(true)) // formatOnFail = true + spiffs_initialized = true; + else + SERIAL_ERROR_MSG("SPIFFS mount failed"); +} + +#endif // WIFISUPPORT && WEBSUPPORT +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/spiffs.h b/src/HAL/ESP32/spiffs.h new file mode 100644 index 0000000..64ec7dd --- /dev/null +++ b/src/HAL/ESP32/spiffs.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +extern bool spiffs_initialized; + +void spiffs_init(); diff --git a/src/HAL/ESP32/timers.cpp b/src/HAL/ESP32/timers.cpp new file mode 100644 index 0000000..c37ad24 --- /dev/null +++ b/src/HAL/ESP32/timers.cpp @@ -0,0 +1,171 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include +#include +#include +#include +#include + +#include "../../inc/MarlinConfig.h" + +// ------------------------ +// Local defines +// ------------------------ + +#define NUM_HARDWARE_TIMERS 4 + +// ------------------------ +// Private Variables +// ------------------------ + +static timg_dev_t *TG[2] = {&TIMERG0, &TIMERG1}; + +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { + { TIMER_GROUP_0, TIMER_0, STEPPER_TIMER_PRESCALE, stepTC_Handler }, // 0 - Stepper + { TIMER_GROUP_0, TIMER_1, TEMP_TIMER_PRESCALE, tempTC_Handler }, // 1 - Temperature + { TIMER_GROUP_1, TIMER_0, PWM_TIMER_PRESCALE, pwmTC_Handler }, // 2 - PWM + { TIMER_GROUP_1, TIMER_1, TONE_TIMER_PRESCALE, toneTC_Handler }, // 3 - Tone +}; + +// ------------------------ +// Public functions +// ------------------------ + +void IRAM_ATTR timer_isr(void *para) { + const tTimerConfig& timer = timer_config[(int)para]; + + // Retrieve the interrupt status and the counter value + // from the timer that reported the interrupt + uint32_t intr_status = TG[timer.group]->int_st_timers.val; + TG[timer.group]->hw_timer[timer.idx].update = 1; + + // Clear the interrupt + if (intr_status & BIT(timer.idx)) { + switch (timer.idx) { + case TIMER_0: TG[timer.group]->int_clr_timers.t0 = 1; break; + case TIMER_1: TG[timer.group]->int_clr_timers.t1 = 1; break; + case TIMER_MAX: break; + } + } + + timer.fn(); + + // After the alarm has been triggered + // Enable it again so it gets triggered the next time + TG[timer.group]->hw_timer[timer.idx].config.alarm_en = TIMER_ALARM_EN; +} + +/** + * Enable and initialize the timer + * @param timer_num timer number to initialize + * @param frequency frequency of the timer + */ +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + const tTimerConfig timer = timer_config[timer_num]; + + timer_config_t config; + config.divider = timer.divider; + config.counter_dir = TIMER_COUNT_UP; + config.counter_en = TIMER_PAUSE; + config.alarm_en = TIMER_ALARM_EN; + config.intr_type = TIMER_INTR_LEVEL; + config.auto_reload = true; + + // Select and initialize the timer + timer_init(timer.group, timer.idx, &config); + + // Timer counter initial value and auto reload on alarm + timer_set_counter_value(timer.group, timer.idx, 0x00000000ULL); + + // Configure the alam value and the interrupt on alarm + timer_set_alarm_value(timer.group, timer.idx, (HAL_TIMER_RATE) / timer.divider / frequency - 1); + + timer_enable_intr(timer.group, timer.idx); + + timer_isr_register(timer.group, timer.idx, timer_isr, (void*)(uint32_t)timer_num, 0, nullptr); + + timer_start(timer.group, timer.idx); +} + +/** + * Set the upper value of the timer, when the timer reaches this upper value the + * interrupt should be triggered and the counter reset + * @param timer_num timer number to set the count to + * @param count threshold at which the interrupt is triggered + */ +void HAL_timer_set_compare(const uint8_t timer_num, hal_timer_t count) { + const tTimerConfig timer = timer_config[timer_num]; + timer_set_alarm_value(timer.group, timer.idx, count); +} + +/** + * Get the current upper value of the timer + * @param timer_num timer number to get the count from + * @return the timer current threshold for the alarm to be triggered + */ +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + const tTimerConfig timer = timer_config[timer_num]; + + uint64_t alarm_value; + timer_get_alarm_value(timer.group, timer.idx, &alarm_value); + + return alarm_value; +} + +/** + * Get the current counter value between 0 and the maximum count (HAL_timer_set_count) + * @param timer_num timer number to get the current count + * @return the current counter of the alarm + */ +hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + const tTimerConfig timer = timer_config[timer_num]; + uint64_t counter_value; + timer_get_counter_value(timer.group, timer.idx, &counter_value); + return counter_value; +} + +/** + * Enable timer interrupt on the timer + * @param timer_num timer number to enable interrupts on + */ +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + //const tTimerConfig timer = timer_config[timer_num]; + //timer_enable_intr(timer.group, timer.idx); +} + +/** + * Disable timer interrupt on the timer + * @param timer_num timer number to disable interrupts on + */ +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + //const tTimerConfig timer = timer_config[timer_num]; + //timer_disable_intr(timer.group, timer.idx); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + const tTimerConfig timer = timer_config[timer_num]; + return TG[timer.group]->int_ena.val | BIT(timer_num); +} + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/timers.h b/src/HAL/ESP32/timers.h new file mode 100644 index 0000000..aa4e155 --- /dev/null +++ b/src/HAL/ESP32/timers.h @@ -0,0 +1,140 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include + +// ------------------------ +// Defines +// ------------------------ +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint64_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFFFFFFFFFFULL + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif +#ifndef MF_TIMER_PWM + #define MF_TIMER_PWM 2 // index of timer to use for PWM outputs +#endif +#ifndef MF_TIMER_TONE + #define MF_TIMER_TONE 3 // index of timer for beeper tones +#endif + +#define HAL_TIMER_RATE APB_CLK_FREQ // frequency of timer peripherals + +#if ENABLED(I2S_STEPPER_STREAM) + #define STEPPER_TIMER_PRESCALE 1 + #define STEPPER_TIMER_RATE 250000 // 250khz, 4µs pulses of i2s word clock + #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs // wrong would be 0.25 +#else + #define STEPPER_TIMER_PRESCALE 40 + #define STEPPER_TIMER_RATE ((HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE)) // frequency of stepper timer, 2MHz + #define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#endif + +#define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts + +#define TONE_TIMER_PRESCALE 1000 // Arbitrary value, no idea what i'm doing here + +#define TEMP_TIMER_PRESCALE 1000 // prescaler for setting Temp timer, 72Khz +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define PWM_TIMER_PRESCALE 10 +#if ENABLED(FAST_PWM_FAN) + #define PWM_TIMER_FREQUENCY FAST_PWM_FAN_FREQUENCY +#else + #define PWM_TIMER_FREQUENCY (50*128) // 50Hz and 7bit resolution +#endif +#define MAX_PWM_PINS 32 // Number of PWM pin-slots + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void tempTC_Handler() +#endif +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void stepTC_Handler() +#endif +#ifndef HAL_PWM_TIMER_ISR + #define HAL_PWM_TIMER_ISR() extern "C" void pwmTC_Handler() +#endif +#ifndef HAL_TONE_TIMER_ISR + #define HAL_TONE_TIMER_ISR() extern "C" void toneTC_Handler() +#endif + +extern "C" { + void tempTC_Handler(); + void stepTC_Handler(); + void pwmTC_Handler(); + void toneTC_Handler(); +} + +// ------------------------ +// Types +// ------------------------ + +typedef struct { + timer_group_t group; + timer_idx_t idx; + uint32_t divider; + void (*fn)(); +} tTimerConfig; + +// ------------------------ +// Public Variables +// ------------------------ + +extern const tTimerConfig timer_config[]; + +// ------------------------ +// Public functions +// ------------------------ + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); +void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t count); +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num); +hal_timer_t HAL_timer_get_count(const uint8_t timer_num); + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/ESP32/u8g_esp32_spi.cpp b/src/HAL/ESP32/u8g_esp32_spi.cpp new file mode 100644 index 0000000..a445035 --- /dev/null +++ b/src/HAL/ESP32/u8g_esp32_spi.cpp @@ -0,0 +1,94 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * Copypaste of SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfig.h" + +#if EITHER(MKS_MINI_12864, FYSETC_MINI_12864_2_1) + +#include +#include "../shared/HAL_SPI.h" +#include "HAL.h" +#include "SPI.h" + +static SPISettings spiConfig; + + +#ifndef LCD_SPI_SPEED + #ifdef SD_SPI_SPEED + #define LCD_SPI_SPEED SD_SPI_SPEED // Assume SPI speed shared with SD + #else + #define LCD_SPI_SPEED SPI_FULL_SPEED // Use full speed if SD speed is not supplied + #endif +#endif + +uint8_t u8g_eps_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + static uint8_t msgInitCount = 2; // Ignore all messages until 2nd U8G_COM_MSG_INIT + if (msgInitCount) { + if (msg == U8G_COM_MSG_INIT) msgInitCount--; + if (msgInitCount) return -1; + } + + switch (msg) { + case U8G_COM_MSG_STOP: break; + + case U8G_COM_MSG_INIT: + OUT_WRITE(DOGLCD_CS, HIGH); + OUT_WRITE(DOGLCD_A0, HIGH); + OUT_WRITE(LCD_RESET_PIN, HIGH); + u8g_Delay(5); + spiBegin(); + spiInit(LCD_SPI_SPEED); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + WRITE(DOGLCD_A0, arg_val ? HIGH : LOW); + break; + + case U8G_COM_MSG_CHIP_SELECT: /* arg_val == 0 means HIGH level of U8G_PI_CS */ + WRITE(DOGLCD_CS, arg_val ? LOW : HIGH); + break; + + case U8G_COM_MSG_RESET: + WRITE(LCD_RESET_PIN, arg_val); + break; + + case U8G_COM_MSG_WRITE_BYTE: + spiSend((uint8_t)arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + break; + } + return 1; +} + +#endif // EITHER(MKS_MINI_12864, FYSETC_MINI_12864_2_1) + +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/web.cpp b/src/HAL/ESP32/web.cpp new file mode 100644 index 0000000..7a27707 --- /dev/null +++ b/src/HAL/ESP32/web.cpp @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(WIFISUPPORT, WEBSUPPORT) + +#include "../../inc/MarlinConfig.h" + +#undef DISABLED // esp32-hal-gpio.h +#include +#include "wifi.h" + +AsyncEventSource events("/events"); // event source (Server-Sent events) + +void onNotFound(AsyncWebServerRequest *request) { + request->send(404); +} + +void web_init() { + server.addHandler(&events); // attach AsyncEventSource + server.serveStatic("/", SPIFFS, "/www").setDefaultFile("index.html"); + server.onNotFound(onNotFound); +} + +#endif // WIFISUPPORT && WEBSUPPORT +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/web.h b/src/HAL/ESP32/web.h new file mode 100644 index 0000000..60023ac --- /dev/null +++ b/src/HAL/ESP32/web.h @@ -0,0 +1,24 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void web_init(); diff --git a/src/HAL/ESP32/wifi.cpp b/src/HAL/ESP32/wifi.cpp new file mode 100644 index 0000000..060f3bd --- /dev/null +++ b/src/HAL/ESP32/wifi.cpp @@ -0,0 +1,66 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../core/serial.h" +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(WIFISUPPORT) + +#include +#include +#include +#include "wifi.h" + +AsyncWebServer server(80); + +#ifndef WIFI_HOSTNAME + #define WIFI_HOSTNAME DEFAULT_WIFI_HOSTNAME +#endif + +void wifi_init() { + + SERIAL_ECHO_MSG("Starting WiFi..."); + + WiFi.mode(WIFI_STA); + WiFi.begin(WIFI_SSID, WIFI_PWD); + + while (WiFi.waitForConnectResult() != WL_CONNECTED) { + SERIAL_ERROR_MSG("Unable to connect to WiFi with SSID '" WIFI_SSID "', restarting."); + delay(5000); + ESP.restart(); + } + + delay(10); + if (!MDNS.begin(WIFI_HOSTNAME)) { + SERIAL_ERROR_MSG("Unable to start mDNS with hostname '" WIFI_HOSTNAME "', restarting."); + delay(5000); + ESP.restart(); + } + + MDNS.addService("http", "tcp", 80); + + SERIAL_ECHOLNPGM("Successfully connected to WiFi with SSID '" WIFI_SSID "', hostname: '" WIFI_HOSTNAME "', IP address: ", WiFi.localIP().toString().c_str()); +} + +#endif // WIFISUPPORT +#endif // ARDUINO_ARCH_ESP32 diff --git a/src/HAL/ESP32/wifi.h b/src/HAL/ESP32/wifi.h new file mode 100644 index 0000000..759a73b --- /dev/null +++ b/src/HAL/ESP32/wifi.h @@ -0,0 +1,30 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +extern AsyncWebServer server; + +#define DEFAULT_WIFI_HOSTNAME "marlin" + +void wifi_init(); diff --git a/src/HAL/HAL.h b/src/HAL/HAL.h new file mode 100644 index 0000000..5186578 --- /dev/null +++ b/src/HAL/HAL.h @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "platforms.h" + +#ifndef GCC_VERSION + #define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) +#endif + +#include HAL_PATH(.,HAL.h) +extern MarlinHAL hal; + +#define HAL_ADC_RANGE _BV(HAL_ADC_RESOLUTION) + +#ifndef I2C_ADDRESS + #define I2C_ADDRESS(A) uint8_t(A) +#endif + +// Needed for AVR sprintf_P PROGMEM extension +#ifndef S_FMT + #define S_FMT "%s" +#endif + +// String helper +#ifndef PGMSTR + #define PGMSTR(NAM,STR) const char NAM[] = STR +#endif diff --git a/src/HAL/LINUX/HAL.cpp b/src/HAL/LINUX/HAL.cpp new file mode 100644 index 0000000..db43f42 --- /dev/null +++ b/src/HAL/LINUX/HAL.cpp @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "../../inc/MarlinConfig.h" +#include "../shared/Delay.h" + +// ------------------------ +// Serial ports +// ------------------------ + +MSerialT usb_serial(TERN0(EMERGENCY_PARSER, true)); + +// U8glib required functions +extern "C" { + void u8g_xMicroDelay(uint16_t val) { DELAY_US(val); } + void u8g_MicroDelay() { u8g_xMicroDelay(1); } + void u8g_10MicroDelay() { u8g_xMicroDelay(10); } + void u8g_Delay(uint16_t val) { delay(val); } +} + +//************************// + +// return free heap space +int freeMemory() { return 0; } + +// ------------------------ +// ADC +// ------------------------ + +uint8_t MarlinHAL::active_ch = 0; + +uint16_t MarlinHAL::adc_value() { + const pin_t pin = analogInputToDigitalPin(active_ch); + if (!VALID_PIN(pin)) return 0; + const uint16_t data = ((Gpio::get(pin) >> 2) & 0x3FF); + return data; // return 10bit value as Marlin expects +} + +void MarlinHAL::reboot() { /* Reset the application state and GPIO */ } + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/HAL.h b/src/HAL/LINUX/HAL.h new file mode 100644 index 0000000..22c3e52 --- /dev/null +++ b/src/HAL/LINUX/HAL.h @@ -0,0 +1,165 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfigPre.h" + +#include +#include +#include +#undef min +#undef max +#include + +#include "hardware/Clock.h" +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "serial.h" + +// ------------------------ +// Defines +// ------------------------ + +#define CPU_32_BIT +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +#define F_CPU 100000000UL +#define SystemCoreClock F_CPU + +#define DELAY_CYCLES(x) Clock::delayCycles(x) + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +void _printf(const char *format, ...); +void _putc(uint8_t c); +uint8_t _getc(); + +//arduino: Print.h +#define DEC 10 +#define HEX 16 +#define OCT 8 +#define BIN 2 +//arduino: binary.h (weird defines) +#define B01 1 +#define B10 2 + +// ------------------------ +// Serial ports +// ------------------------ + +extern MSerialT usb_serial; +#define MYSERIAL1 usb_serial + +// +// Interrupts +// +#define CRITICAL_SECTION_START() +#define CRITICAL_SECTION_END() + +// ADC +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() {} + static void watchdog_refresh() {} + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Reset the application state and GPIO + + // Interrupts + static bool isr_state() { return true; } + static void isr_on() {} + static void isr_off() {} + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static constexpr uint8_t reset_reason = RST_POWER_ON; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint8_t active_ch; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t) {} + + // Begin ADC sampling on the given channel + static void adc_start(const uint8_t ch) { active_ch = ch; } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to change the resolution or invert the duty cycle. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + + static void set_pwm_frequency(const pin_t, int) {} +}; diff --git a/src/HAL/LINUX/MarlinSPI.h b/src/HAL/LINUX/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/LINUX/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/LINUX/arduino.cpp b/src/HAL/LINUX/arduino.cpp new file mode 100644 index 0000000..075b4cc --- /dev/null +++ b/src/HAL/LINUX/arduino.cpp @@ -0,0 +1,99 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include +#include "../../inc/MarlinConfig.h" +#include "hardware/Clock.h" +#include "../shared/Delay.h" + +// Interrupts +void cli() { } // Disable +void sei() { } // Enable + +// Time functions +void _delay_ms(const int ms) { delay(ms); } + +uint32_t millis() { + return (uint32_t)Clock::millis(); +} + +// This is required for some Arduino libraries we are using +void delayMicroseconds(uint32_t us) { + Clock::delayMicros(us); +} + +extern "C" void delay(const int msec) { + Clock::delayMillis(msec); +} + +// IO functions +// As defined by Arduino INPUT(0x0), OUTPUT(0x1), INPUT_PULLUP(0x2) +void pinMode(const pin_t pin, const uint8_t mode) { + if (!VALID_PIN(pin)) return; + Gpio::setMode(pin, mode); +} + +void digitalWrite(pin_t pin, uint8_t pin_status) { + if (!VALID_PIN(pin)) return; + Gpio::set(pin, pin_status); +} + +bool digitalRead(pin_t pin) { + if (!VALID_PIN(pin)) return false; + return Gpio::get(pin); +} + +void analogWrite(pin_t pin, int pwm_value) { // 1 - 254: pwm_value, 0: LOW, 255: HIGH + if (!VALID_PIN(pin)) return; + Gpio::set(pin, pwm_value); +} + +uint16_t analogRead(pin_t adc_pin) { + if (!VALID_PIN(DIGITAL_PIN_TO_ANALOG_PIN(adc_pin))) return 0; + return Gpio::get(DIGITAL_PIN_TO_ANALOG_PIN(adc_pin)); +} + +char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s) { + char format_string[20]; + snprintf(format_string, 20, "%%%d.%df", __width, __prec); + sprintf(__s, format_string, __val); + return __s; +} + +int32_t random(int32_t max) { + return rand() % max; +} + +int32_t random(int32_t min, int32_t max) { + return min + rand() % (max - min); +} + +void randomSeed(uint32_t value) { + srand(value); +} + +int map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max) { + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/eeprom.cpp b/src/HAL/LINUX/eeprom.cpp new file mode 100644 index 0000000..f878bba --- /dev/null +++ b/src/HAL/LINUX/eeprom.cpp @@ -0,0 +1,104 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(EEPROM_SETTINGS) + +#include "../shared/eeprom_api.h" +#include + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB of Emulated EEPROM +#endif + +uint8_t buffer[MARLIN_EEPROM_SIZE]; +char filename[] = "eeprom.dat"; + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { + const char eeprom_erase_value = 0xFF; + FILE * eeprom_file = fopen(filename, "rb"); + if (!eeprom_file) return false; + + fseek(eeprom_file, 0L, SEEK_END); + std::size_t file_size = ftell(eeprom_file); + + if (file_size < MARLIN_EEPROM_SIZE) { + memset(buffer + file_size, eeprom_erase_value, MARLIN_EEPROM_SIZE - file_size); + } + else { + fseek(eeprom_file, 0L, SEEK_SET); + fread(buffer, sizeof(uint8_t), sizeof(buffer), eeprom_file); + } + + fclose(eeprom_file); + return true; +} + +bool PersistentStore::access_finish() { + FILE * eeprom_file = fopen(filename, "wb"); + if (!eeprom_file) return false; + fwrite(buffer, sizeof(uint8_t), sizeof(buffer), eeprom_file); + fclose(eeprom_file); + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + std::size_t bytes_written = 0; + + for (std::size_t i = 0; i < size; i++) { + buffer[pos + i] = value[i]; + bytes_written++; + } + + crc16(crc, value, size); + pos += size; + return (bytes_written != size); // return true for any error +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uint16_t *crc, const bool writing/*=true*/) { + std::size_t bytes_read = 0; + if (writing) { + for (std::size_t i = 0; i < size; i++) { + value[i] = buffer[pos + i]; + bytes_read++; + } + crc16(crc, value, size); + } + else { + uint8_t temp[size]; + for (std::size_t i = 0; i < size; i++) { + temp[i] = buffer[pos + i]; + bytes_read++; + } + crc16(crc, temp, size); + } + + pos += size; + return bytes_read != size; // return true for any error +} + +#endif // EEPROM_SETTINGS +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/fastio.h b/src/HAL/LINUX/fastio.h new file mode 100644 index 0000000..4567c62 --- /dev/null +++ b/src/HAL/LINUX/fastio.h @@ -0,0 +1,111 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for X86_64 + */ + +#include "../shared/Marduino.h" +#include + +#define SET_DIR_INPUT(IO) Gpio::setDir(IO, 1) +#define SET_DIR_OUTPUT(IO) Gpio::setDir(IO, 0) + +#define SET_MODE(IO, mode) Gpio::setMode(IO, mode) + +#define WRITE_PIN_SET(IO) Gpio::set(IO) +#define WRITE_PIN_CLR(IO) Gpio::clear(IO) + +#define READ_PIN(IO) Gpio::get(IO) +#define WRITE_PIN(IO,V) Gpio::set(IO, V) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(STEP); WRITE(STEP, HIGH); WRITE(STEP, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/gcc-4.8.5/cpp/Stringification.html + */ + +/// Read a pin +#define _READ(IO) READ_PIN(IO) + +/// Write to a pin +#define _WRITE(IO,V) WRITE_PIN(IO,V) + +/// toggle a pin +#define _TOGGLE(IO) _WRITE(IO, !READ(IO)) + +/// set pin as input +#define _SET_INPUT(IO) SET_DIR_INPUT(IO) + +/// set pin as output +#define _SET_OUTPUT(IO) SET_DIR_OUTPUT(IO) + +/// set pin as input with pullup mode +#define _PULLUP(IO,V) pinMode(IO, (V) ? INPUT_PULLUP : INPUT) + +/// set pin as input with pulldown mode +#define _PULLDOWN(IO,V) pinMode(IO, (V) ? INPUT_PULLDOWN : INPUT) + +// hg42: all pins can be input or output (I hope) +// hg42: undefined pins create compile error (IO, is no pin) +// hg42: currently not used, but was used by pinsDebug + +/// check if pin is an input +#define _IS_INPUT(IO) (LPC1768_PIN_PIN(IO) >= 0) + +/// check if pin is an output +#define _IS_OUTPUT(IO) (LPC1768_PIN_PIN(IO) >= 0) + +/// Read a pin wrapper +#define READ(IO) _READ(IO) + +/// Write to a pin wrapper +#define WRITE(IO,V) _WRITE(IO,V) + +/// toggle a pin wrapper +#define TOGGLE(IO) _TOGGLE(IO) + +/// set pin as input wrapper +#define SET_INPUT(IO) _SET_INPUT(IO) +/// set pin as input with pullup wrapper +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) +/// set pin as input with pulldown wrapper +#define SET_INPUT_PULLDOWN(IO) do{ _SET_INPUT(IO); _PULLDOWN(IO, HIGH); }while(0) +/// set pin as output wrapper - reads the pin and sets the output to that value +#define SET_OUTPUT(IO) do{ _WRITE(IO, _READ(IO)); _SET_OUTPUT(IO); }while(0) +// set pin as PWM +#define SET_PWM(IO) SET_OUTPUT(IO) + +/// check if pin is an input wrapper +#define IS_INPUT(IO) _IS_INPUT(IO) +/// check if pin is an output wrapper +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/src/HAL/LINUX/hardware/Clock.cpp b/src/HAL/LINUX/hardware/Clock.cpp new file mode 100644 index 0000000..1984a4a --- /dev/null +++ b/src/HAL/LINUX/hardware/Clock.cpp @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "../../../inc/MarlinConfig.h" +#include "Clock.h" + +std::chrono::nanoseconds Clock::startup = std::chrono::high_resolution_clock::now().time_since_epoch(); +uint32_t Clock::frequency = F_CPU; +double Clock::time_multiplier = 1.0; + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/Clock.h b/src/HAL/LINUX/hardware/Clock.h new file mode 100644 index 0000000..072eacf --- /dev/null +++ b/src/HAL/LINUX/hardware/Clock.h @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include + +class Clock { +public: + static uint64_t ticks(uint32_t frequency = Clock::frequency) { + return (Clock::nanos() - Clock::startup.count()) / (1000000000ULL / frequency); + } + + static uint64_t nanosToTicks(uint64_t ns, uint32_t frequency = Clock::frequency) { + return ns / (1000000000ULL / frequency); + } + + // Time acceleration compensated + static uint64_t ticksToNanos(uint64_t tick, uint32_t frequency = Clock::frequency) { + return (tick * (1000000000ULL / frequency)) / Clock::time_multiplier; + } + + static void setFrequency(uint32_t freq) { + Clock::frequency = freq; + } + + // Time Acceleration compensated + static uint64_t nanos() { + auto now = std::chrono::high_resolution_clock::now().time_since_epoch(); + return (now.count() - Clock::startup.count()) * Clock::time_multiplier; + } + + static uint64_t micros() { + return Clock::nanos() / 1000; + } + + static uint64_t millis() { + return Clock::micros() / 1000; + } + + static double seconds() { + return Clock::nanos() / 1000000000.0; + } + + static void delayCycles(uint64_t cycles) { + std::this_thread::sleep_for(std::chrono::nanoseconds( (1000000000L / frequency) * cycles) / Clock::time_multiplier ); + } + + static void delayMicros(uint64_t micros) { + std::this_thread::sleep_for(std::chrono::microseconds( micros ) / Clock::time_multiplier); + } + + static void delayMillis(uint64_t millis) { + std::this_thread::sleep_for(std::chrono::milliseconds( millis ) / Clock::time_multiplier); + } + + static void delaySeconds(double secs) { + std::this_thread::sleep_for(std::chrono::duration(secs * 1000) / Clock::time_multiplier); + } + + // Will reduce timer resolution increasing likelihood of overflows + static void setTimeMultiplier(double tm) { + Clock::time_multiplier = tm; + } + +private: + static std::chrono::nanoseconds startup; + static uint32_t frequency; + static double time_multiplier; +}; diff --git a/src/HAL/LINUX/hardware/Gpio.cpp b/src/HAL/LINUX/hardware/Gpio.cpp new file mode 100644 index 0000000..61a7be7 --- /dev/null +++ b/src/HAL/LINUX/hardware/Gpio.cpp @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "Gpio.h" + +pin_data Gpio::pin_map[Gpio::pin_count+1] = {}; +IOLogger* Gpio::logger = nullptr; + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/Gpio.h b/src/HAL/LINUX/hardware/Gpio.h new file mode 100644 index 0000000..f946be6 --- /dev/null +++ b/src/HAL/LINUX/hardware/Gpio.h @@ -0,0 +1,141 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "Clock.h" +#include "../../../inc/MarlinConfigPre.h" +#include + +typedef int16_t pin_type; + +struct GpioEvent { + enum Type { + NOP, + FALL, + RISE, + SET_VALUE, + SETM, + SETD + }; + uint64_t timestamp; + pin_type pin_id; + GpioEvent::Type event; + + GpioEvent(uint64_t timestamp, pin_type pin_id, GpioEvent::Type event) { + this->timestamp = timestamp; + this->pin_id = pin_id; + this->event = event; + } +}; + +class IOLogger { +public: + virtual ~IOLogger(){}; + virtual void log(GpioEvent ev) = 0; +}; + +class Peripheral { +public: + virtual ~Peripheral(){}; + virtual void interrupt(GpioEvent ev) = 0; + virtual void update() = 0; +}; + +struct pin_data { + uint8_t dir; + uint8_t mode; + uint16_t value; + Peripheral* cb; +}; + +class Gpio { +public: + + static const pin_type pin_count = 255; + static pin_data pin_map[pin_count+1]; + + static bool valid_pin(pin_type pin) { + return pin >= 0 && pin <= pin_count; + } + + static void set(pin_type pin) { + set(pin, 1); + } + + static void set(pin_type pin, uint16_t value) { + if (!valid_pin(pin)) return; + GpioEvent::Type evt_type = value > 1 ? GpioEvent::SET_VALUE : value > pin_map[pin].value ? GpioEvent::RISE : value < pin_map[pin].value ? GpioEvent::FALL : GpioEvent::NOP; + pin_map[pin].value = value; + GpioEvent evt(Clock::nanos(), pin, evt_type); + if (pin_map[pin].cb) { + pin_map[pin].cb->interrupt(evt); + } + if (Gpio::logger) Gpio::logger->log(evt); + } + + static uint16_t get(pin_type pin) { + if (!valid_pin(pin)) return 0; + return pin_map[pin].value; + } + + static void clear(pin_type pin) { + set(pin, 0); + } + + static void setMode(pin_type pin, uint8_t value) { + if (!valid_pin(pin)) return; + pin_map[pin].mode = value; + GpioEvent evt(Clock::nanos(), pin, GpioEvent::Type::SETM); + if (pin_map[pin].cb) pin_map[pin].cb->interrupt(evt); + if (Gpio::logger) Gpio::logger->log(evt); + } + + static uint8_t getMode(pin_type pin) { + if (!valid_pin(pin)) return 0; + return pin_map[pin].mode; + } + + static void setDir(pin_type pin, uint8_t value) { + if (!valid_pin(pin)) return; + pin_map[pin].dir = value; + GpioEvent evt(Clock::nanos(), pin, GpioEvent::Type::SETD); + if (pin_map[pin].cb) pin_map[pin].cb->interrupt(evt); + if (Gpio::logger) Gpio::logger->log(evt); + } + + static uint8_t getDir(pin_type pin) { + if (!valid_pin(pin)) return 0; + return pin_map[pin].dir; + } + + static void attachPeripheral(pin_type pin, Peripheral* per) { + if (!valid_pin(pin)) return; + pin_map[pin].cb = per; + } + + static void attachLogger(IOLogger* logger) { + Gpio::logger = logger; + } + +private: + static IOLogger* logger; +}; diff --git a/src/HAL/LINUX/hardware/Heater.cpp b/src/HAL/LINUX/hardware/Heater.cpp new file mode 100644 index 0000000..44f1198 --- /dev/null +++ b/src/HAL/LINUX/hardware/Heater.cpp @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "Clock.h" +#include +#include "../../../inc/MarlinConfig.h" + +#include "Heater.h" + +Heater::Heater(pin_t heater, pin_t adc) { + heater_state = 0; + room_temp_raw = 150; + last = Clock::micros(); + heater_pin = heater; + adc_pin = adc; + heat = 0.0; +} + +Heater::~Heater() { +} + +void Heater::update() { + // crude pwm read and cruder heat simulation + auto now = Clock::micros(); + double delta = (now - last); + if (delta > 1000 ) { + heater_state = pwmcap.update(0xFFFF * Gpio::pin_map[heater_pin].value); + last = now; + heat += (heater_state - heat) * (delta / 1000000000.0); + + NOLESS(heat, room_temp_raw); + Gpio::pin_map[analogInputToDigitalPin(adc_pin)].value = 0xFFFF - (uint16_t)heat; + } +} + +void Heater::interrupt(GpioEvent ev) { + // unused +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/Heater.h b/src/HAL/LINUX/hardware/Heater.h new file mode 100644 index 0000000..6d590ce --- /dev/null +++ b/src/HAL/LINUX/hardware/Heater.h @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "Gpio.h" + +struct LowpassFilter { + uint64_t data_delay = 0; + uint16_t update(uint16_t value) { + data_delay += value - (data_delay >> 6); + return uint16_t(data_delay >> 6); + } +}; + +class Heater: public Peripheral { +public: + Heater(pin_t heater, pin_t adc); + virtual ~Heater(); + void interrupt(GpioEvent ev); + void update(); + + pin_t heater_pin, adc_pin; + uint16_t room_temp_raw; + uint16_t heater_state; + LowpassFilter pwmcap; + double heat; + uint64_t last; +}; diff --git a/src/HAL/LINUX/hardware/IOLoggerCSV.cpp b/src/HAL/LINUX/hardware/IOLoggerCSV.cpp new file mode 100644 index 0000000..c11fd1f --- /dev/null +++ b/src/HAL/LINUX/hardware/IOLoggerCSV.cpp @@ -0,0 +1,49 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "IOLoggerCSV.h" + +IOLoggerCSV::IOLoggerCSV(std::string filename) { + file.open(filename); +} + +IOLoggerCSV::~IOLoggerCSV() { + file.close(); +} + +void IOLoggerCSV::log(GpioEvent ev) { + std::lock_guard lock(vector_lock); + events.push_back(ev); //minimal impact to signal handler +} + +void IOLoggerCSV::flush() { + { std::lock_guard lock(vector_lock); + while (!events.empty()) { + file << events.front().timestamp << ", "<< events.front().pin_id << ", " << events.front().event << std::endl; + events.pop_front(); + } + } + file.flush(); +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/IOLoggerCSV.h b/src/HAL/LINUX/hardware/IOLoggerCSV.h new file mode 100644 index 0000000..d8fe738 --- /dev/null +++ b/src/HAL/LINUX/hardware/IOLoggerCSV.h @@ -0,0 +1,40 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include +#include +#include "Gpio.h" + +class IOLoggerCSV: public IOLogger { +public: + IOLoggerCSV(std::string filename); + virtual ~IOLoggerCSV(); + void flush(); + void log(GpioEvent ev); + +private: + std::ofstream file; + std::list events; + std::mutex vector_lock; +}; diff --git a/src/HAL/LINUX/hardware/LinearAxis.cpp b/src/HAL/LINUX/hardware/LinearAxis.cpp new file mode 100644 index 0000000..e122ef3 --- /dev/null +++ b/src/HAL/LINUX/hardware/LinearAxis.cpp @@ -0,0 +1,65 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include +#include +#include "Clock.h" +#include "LinearAxis.h" + +LinearAxis::LinearAxis(pin_type enable, pin_type dir, pin_type step, pin_type end_min, pin_type end_max) { + enable_pin = enable; + dir_pin = dir; + step_pin = step; + min_pin = end_min; + max_pin = end_max; + + min_position = 50; + max_position = (200*80) + min_position; + position = rand() % ((max_position - 40) - min_position) + (min_position + 20); + last_update = Clock::nanos(); + + Gpio::attachPeripheral(step_pin, this); + +} + +LinearAxis::~LinearAxis() { + +} + +void LinearAxis::update() { + +} + +void LinearAxis::interrupt(GpioEvent ev) { + if (ev.pin_id == step_pin && !Gpio::pin_map[enable_pin].value) { + if (ev.event == GpioEvent::RISE) { + last_update = ev.timestamp; + position += -1 + 2 * Gpio::pin_map[dir_pin].value; + Gpio::pin_map[min_pin].value = (position < min_position); + //Gpio::pin_map[max_pin].value = (position > max_position); + //if (position < min_position) printf("axis(%d) endstop : pos: %d, mm: %f, min: %d\n", step_pin, position, position / 80.0, Gpio::pin_map[min_pin].value); + } + } +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/LinearAxis.h b/src/HAL/LINUX/hardware/LinearAxis.h new file mode 100644 index 0000000..34541e7 --- /dev/null +++ b/src/HAL/LINUX/hardware/LinearAxis.h @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include "Gpio.h" + +class LinearAxis: public Peripheral { +public: + LinearAxis(pin_type enable, pin_type dir, pin_type step, pin_type end_min, pin_type end_max); + virtual ~LinearAxis(); + void update(); + void interrupt(GpioEvent ev); + + pin_type enable_pin; + pin_type dir_pin; + pin_type step_pin; + pin_type min_pin; + pin_type max_pin; + + int32_t position; + int32_t min_position; + int32_t max_position; + uint64_t last_update; + +}; diff --git a/src/HAL/LINUX/hardware/Timer.cpp b/src/HAL/LINUX/hardware/Timer.cpp new file mode 100644 index 0000000..9f0d6a8 --- /dev/null +++ b/src/HAL/LINUX/hardware/Timer.cpp @@ -0,0 +1,117 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "Timer.h" +#include + +Timer::Timer() { + active = false; + compare = 0; + frequency = 0; + overruns = 0; + timerid = 0; + cbfn = nullptr; + period = 0; + start_time = 0; + avg_error = 0; +} + +Timer::~Timer() { + timer_delete(timerid); +} + +void Timer::init(uint32_t sig_id, uint32_t sim_freq, callback_fn* fn) { + struct sigaction sa; + struct sigevent sev; + + frequency = sim_freq; + cbfn = fn; + + sa.sa_flags = SA_SIGINFO; + sa.sa_sigaction = Timer::handler; + sigemptyset(&sa.sa_mask); + if (sigaction(SIGRTMIN, &sa, nullptr) == -1) { + return; // todo: handle error + } + + sigemptyset(&mask); + sigaddset(&mask, SIGRTMIN); + + disable(); + + sev.sigev_notify = SIGEV_SIGNAL; + sev.sigev_signo = SIGRTMIN; + sev.sigev_value.sival_ptr = (void*)this; + if (timer_create(CLOCK_REALTIME, &sev, &timerid) == -1) { + return; // todo: handle error + } +} + +void Timer::start(uint32_t frequency) { + setCompare(this->frequency / frequency); + //printf("timer(%ld) started\n", getID()); +} + +void Timer::enable() { + if (sigprocmask(SIG_UNBLOCK, &mask, nullptr) == -1) { + return; // todo: handle error + } + active = true; + //printf("timer(%ld) enabled\n", getID()); +} + +void Timer::disable() { + if (sigprocmask(SIG_SETMASK, &mask, nullptr) == -1) { + return; // todo: handle error + } + active = false; +} + +void Timer::setCompare(uint32_t compare) { + uint32_t nsec_offset = 0; + if (active) { + nsec_offset = Clock::nanos() - this->start_time; // calculate how long the timer would have been running for + nsec_offset = nsec_offset < 1000 ? nsec_offset : 0; // constrain, this shouldn't be needed but apparently Marlin enables interrupts on the stepper timer before initialising it, todo: investigate ?bug? + } + this->compare = compare; + uint64_t ns = Clock::ticksToNanos(compare, frequency) - nsec_offset; + struct itimerspec its; + its.it_value.tv_sec = ns / 1000000000; + its.it_value.tv_nsec = ns % 1000000000; + its.it_interval.tv_sec = its.it_value.tv_sec; + its.it_interval.tv_nsec = its.it_value.tv_nsec; + + if (timer_settime(timerid, 0, &its, nullptr) == -1) { + printf("timer(%ld) failed, compare: %d(%ld)\n", getID(), compare, its.it_value.tv_nsec); + return; // todo: handle error + } + //printf("timer(%ld) started, compare: %d(%d)\n", getID(), compare, its.it_value.tv_nsec); + this->period = its.it_value.tv_nsec; + this->start_time = Clock::nanos(); +} + +uint32_t Timer::getCount() { + return Clock::nanosToTicks(Clock::nanos() - this->start_time, frequency); +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/hardware/Timer.h b/src/HAL/LINUX/hardware/Timer.h new file mode 100644 index 0000000..1b3b800 --- /dev/null +++ b/src/HAL/LINUX/hardware/Timer.h @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "Clock.h" + +class Timer { +public: + Timer(); + virtual ~Timer(); + + typedef void (callback_fn)(); + + void init(uint32_t sig_id, uint32_t sim_freq, callback_fn* fn); + void start(uint32_t frequency); + void enable(); + bool enabled() {return active;} + void disable(); + void setCompare(uint32_t compare); + uint32_t getCount(); + uint32_t getCompare() {return compare;} + uint32_t getOverruns() {return overruns;} + uint32_t getAvgError() {return avg_error;} + + intptr_t getID() { + return (*(intptr_t*)timerid); + } + + static void handler(int sig, siginfo_t *si, void *uc) { + Timer* _this = (Timer*)si->si_value.sival_ptr; + _this->avg_error += (Clock::nanos() - _this->start_time) - _this->period; //high_resolution_clock is also limited in precision, but best we have + _this->avg_error /= 2; //very crude precision analysis (actually within +-500ns usually) + _this->start_time = Clock::nanos(); // wrap + _this->cbfn(); + _this->overruns += timer_getoverrun(_this->timerid); // even at 50Khz this doesn't stay zero, again demonstrating the limitations + // using a realtime linux kernel would help somewhat + } + +private: + bool active; + uint32_t compare; + uint32_t frequency; + uint32_t overruns; + timer_t timerid; + sigset_t mask; + callback_fn* cbfn; + uint64_t period; + uint64_t avg_error; + uint64_t start_time; +}; diff --git a/src/HAL/LINUX/inc/Conditionals_LCD.h b/src/HAL/LINUX/inc/Conditionals_LCD.h new file mode 100644 index 0000000..99a6fc2 --- /dev/null +++ b/src/HAL/LINUX/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/LINUX." +#endif diff --git a/src/HAL/LINUX/inc/Conditionals_adv.h b/src/HAL/LINUX/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/LINUX/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/LINUX/inc/Conditionals_post.h b/src/HAL/LINUX/inc/Conditionals_post.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/LINUX/inc/Conditionals_post.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/LINUX/inc/SanityCheck.h b/src/HAL/LINUX/inc/SanityCheck.h new file mode 100644 index 0000000..36d3190 --- /dev/null +++ b/src/HAL/LINUX/inc/SanityCheck.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test X86_64-specific configuration values for errors at compile-time. + */ + +// Emulating RAMPS +#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) + #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on LINUX." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on LINUX." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on LINUX." +#endif diff --git a/src/HAL/LINUX/include/Arduino.h b/src/HAL/LINUX/include/Arduino.h new file mode 100644 index 0000000..f05aaed --- /dev/null +++ b/src/HAL/LINUX/include/Arduino.h @@ -0,0 +1,94 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include +#include +#include + +#include + +#define HIGH 0x01 +#define LOW 0x00 + +#define INPUT 0x00 +#define OUTPUT 0x01 +#define INPUT_PULLUP 0x02 +#define INPUT_PULLDOWN 0x03 + +#define LSBFIRST 0 +#define MSBFIRST 1 + +#define CHANGE 0x02 +#define FALLING 0x03 +#define RISING 0x04 + +typedef uint8_t byte; +#define PROGMEM +#define PSTR(v) (v) +#define PGM_P const char * + +// Used for libraries, preprocessor, and constants +#define abs(x) ((x)>0?(x):-(x)) + +#ifndef isnan + #define isnan std::isnan +#endif +#ifndef isinf + #define isinf std::isinf +#endif + +#define sq(v) ((v) * (v)) +#define constrain(value, arg_min, arg_max) ((value) < (arg_min) ? (arg_min) :((value) > (arg_max) ? (arg_max) : (value))) + +// Interrupts +void cli(); // Disable +void sei(); // Enable +void attachInterrupt(uint32_t pin, void (*callback)(), uint32_t mode); +void detachInterrupt(uint32_t pin); + +extern "C" { + void GpioEnableInt(uint32_t port, uint32_t pin, uint32_t mode); + void GpioDisableInt(uint32_t port, uint32_t pin); +} + +// Time functions +extern "C" void delay(const int ms); +void _delay_ms(const int ms); +void delayMicroseconds(unsigned long); +uint32_t millis(); + +//IO functions +void pinMode(const pin_t, const uint8_t); +void digitalWrite(pin_t, uint8_t); +bool digitalRead(pin_t); +void analogWrite(pin_t, int); +uint16_t analogRead(pin_t); + +int32_t random(int32_t); +int32_t random(int32_t, int32_t); +void randomSeed(uint32_t); + +char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s); + +int map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max); diff --git a/src/HAL/LINUX/include/pinmapping.cpp b/src/HAL/LINUX/include/pinmapping.cpp new file mode 100644 index 0000000..5823668 --- /dev/null +++ b/src/HAL/LINUX/include/pinmapping.cpp @@ -0,0 +1,32 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include + +#include "../../../gcode/parser.h" + +int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) { + return parser.intval(code, dval); +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/include/pinmapping.h b/src/HAL/LINUX/include/pinmapping.h new file mode 100644 index 0000000..cfac5e3 --- /dev/null +++ b/src/HAL/LINUX/include/pinmapping.h @@ -0,0 +1,65 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfigPre.h" + +#include +#include "../hardware/Gpio.h" + +typedef pin_type pin_t; + +#define P_NC -1 +constexpr uint16_t NUM_DIGITAL_PINS = Gpio::pin_count; +constexpr uint8_t NUM_ANALOG_INPUTS = 16; + +#define HAL_SENSITIVE_PINS + +constexpr uint8_t analog_offset = NUM_DIGITAL_PINS - NUM_ANALOG_INPUTS; + +// Get the digital pin for an analog index +constexpr pin_t analogInputToDigitalPin(const int8_t p) { + return (WITHIN(p, 0, NUM_ANALOG_INPUTS) ? analog_offset + p : P_NC); +} + +// Get the analog index for a digital pin +constexpr int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p) { + return (WITHIN(p, analog_offset, NUM_DIGITAL_PINS) ? p - analog_offset : P_NC); +} + +// Return the index of a pin number +constexpr int16_t GET_PIN_MAP_INDEX(const pin_t pin) { return pin; } + +// Test whether the pin is valid +constexpr bool VALID_PIN(const pin_t p) { return WITHIN(p, 0, NUM_DIGITAL_PINS); } + +// Test whether the pin is PWM +constexpr bool PWM_PIN(const pin_t p) { return false; } + +// Test whether the pin is interruptible +constexpr bool INTERRUPT_PIN(const pin_t p) { return false; } + +// Get the pin number at the given index +constexpr pin_t GET_PIN_MAP_PIN(const int16_t ind) { return ind; } + +// Parse a G-code word into a pin index +int16_t PARSED_PIN_INDEX(const char code, const int16_t dval); diff --git a/src/HAL/LINUX/include/serial.h b/src/HAL/LINUX/include/serial.h new file mode 100644 index 0000000..ebae066 --- /dev/null +++ b/src/HAL/LINUX/include/serial.h @@ -0,0 +1,118 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfigPre.h" +#if ENABLED(EMERGENCY_PARSER) + #include "../../../feature/e_parser.h" +#endif +#include "../../../core/serial_hook.h" + +#include +#include + +/** + * Generic RingBuffer + * T type of the buffer array + * S size of the buffer (must be power of 2) + */ +template class RingBuffer { +public: + RingBuffer() { index_read = index_write = 0; } + uint32_t available() volatile { return index_write - index_read; } + uint32_t free() volatile { return buffer_size - available(); } + bool empty() volatile { return index_read == index_write; } + bool full() volatile { return available() == buffer_size; } + void clear() volatile { index_read = index_write = 0; } + + bool peek(T *value) volatile { + if (value == 0 || available() == 0) + return false; + *value = buffer[mask(index_read)]; + return true; + } + + int read() volatile { + if (empty()) return -1; + return buffer[mask(index_read++)]; + } + + bool write(T value) volatile { + if (full()) return false; + buffer[mask(index_write++)] = value; + return true; + } + +private: + uint32_t mask(uint32_t val) volatile { + return buffer_mask & val; + } + + static const uint32_t buffer_size = S; + static const uint32_t buffer_mask = buffer_size - 1; + volatile T buffer[buffer_size]; + volatile uint32_t index_write; + volatile uint32_t index_read; +}; + +struct HalSerial { + HalSerial() { host_connected = true; } + + void begin(int32_t) {} + void end() {} + + int peek() { + uint8_t value; + return receive_buffer.peek(&value) ? value : -1; + } + + int read() { return receive_buffer.read(); } + + size_t write(char c) { + if (!host_connected) return 0; + while (!transmit_buffer.free()); + return transmit_buffer.write(c); + } + + bool connected() { return host_connected; } + + uint16_t available() { + return (uint16_t)receive_buffer.available(); + } + + void flush() { receive_buffer.clear(); } + + uint8_t availableForWrite() { + return transmit_buffer.free() > 255 ? 255 : (uint8_t)transmit_buffer.free(); + } + + void flushTX() { + if (host_connected) + while (transmit_buffer.available()) { /* nada */ } + } + + volatile RingBuffer receive_buffer; + volatile RingBuffer transmit_buffer; + volatile bool host_connected; +}; + +typedef Serial1Class MSerialT; diff --git a/src/HAL/LINUX/main.cpp b/src/HAL/LINUX/main.cpp new file mode 100644 index 0000000..f2af2ff --- /dev/null +++ b/src/HAL/LINUX/main.cpp @@ -0,0 +1,138 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef __PLAT_LINUX__ + +//#define GPIO_LOGGING // Full GPIO and Positional Logging + +#include "../../inc/MarlinConfig.h" +#include "../shared/Delay.h" +#include "hardware/IOLoggerCSV.h" +#include "hardware/Heater.h" +#include "hardware/LinearAxis.h" + +#include +#include +#include +#include +#include + +extern void setup(); +extern void loop(); + +// simple stdout / stdin implementation for fake serial port +void write_serial_thread() { + for (;;) { + for (std::size_t i = usb_serial.transmit_buffer.available(); i > 0; i--) { + fputc(usb_serial.transmit_buffer.read(), stdout); + } + std::this_thread::yield(); + } +} + +void read_serial_thread() { + char buffer[255] = {}; + for (;;) { + std::size_t len = _MIN(usb_serial.receive_buffer.free(), 254U); + if (fgets(buffer, len, stdin)) + for (std::size_t i = 0; i < strlen(buffer); i++) + usb_serial.receive_buffer.write(buffer[i]); + std::this_thread::yield(); + } +} + +void simulation_loop() { + Heater hotend(HEATER_0_PIN, TEMP_0_PIN); + Heater bed(HEATER_BED_PIN, TEMP_BED_PIN); + LinearAxis x_axis(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_MIN_PIN, X_MAX_PIN); + LinearAxis y_axis(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_MIN_PIN, Y_MAX_PIN); + LinearAxis z_axis(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_MIN_PIN, Z_MAX_PIN); + LinearAxis extruder0(E0_ENABLE_PIN, E0_DIR_PIN, E0_STEP_PIN, P_NC, P_NC); + + #ifdef GPIO_LOGGING + IOLoggerCSV logger("all_gpio_log.csv"); + Gpio::attachLogger(&logger); + + std::ofstream position_log; + position_log.open("axis_position_log.csv"); + + int32_t x,y,z; + #endif + + for (;;) { + + hotend.update(); + bed.update(); + + x_axis.update(); + y_axis.update(); + z_axis.update(); + extruder0.update(); + + #ifdef GPIO_LOGGING + if (x_axis.position != x || y_axis.position != y || z_axis.position != z) { + uint64_t update = _MAX(x_axis.last_update, y_axis.last_update, z_axis.last_update); + position_log << update << ", " << x_axis.position << ", " << y_axis.position << ", " << z_axis.position << std::endl; + position_log.flush(); + x = x_axis.position; + y = y_axis.position; + z = z_axis.position; + } + // flush the logger + logger.flush(); + #endif + + std::this_thread::yield(); + } +} + +int main() { + std::thread write_serial (write_serial_thread); + std::thread read_serial (read_serial_thread); + + #ifdef MYSERIAL1 + MYSERIAL1.begin(BAUDRATE); + SERIAL_ECHOLNPGM("x86_64 Initialized"); + SERIAL_FLUSHTX(); + #endif + + Clock::setFrequency(F_CPU); + Clock::setTimeMultiplier(1.0); // some testing at 10x + + HAL_timer_init(); + + std::thread simulation (simulation_loop); + + DELAY_US(10000); + + setup(); + for (;;) { + loop(); + std::this_thread::yield(); + } + + simulation.join(); + write_serial.join(); + read_serial.join(); +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/pinsDebug.h b/src/HAL/LINUX/pinsDebug.h new file mode 100644 index 0000000..7bfd97d --- /dev/null +++ b/src/HAL/LINUX/pinsDebug.h @@ -0,0 +1,63 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Support routines for X86_64 + */ + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS +#define pwm_details(pin) NOOP // (do nothing) +#define pwm_status(pin) false // Print a pin's PWM status. Return true if it's currently a PWM pin. +#define IS_ANALOG(P) (DIGITAL_PIN_TO_ANALOG_PIN(P) >= 0 ? 1 : 0) +#define digitalRead_mod(p) digitalRead(p) +#define PRINT_PORT(p) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin + +// active ADC function/mode/code values for PINSEL registers +constexpr int8_t ADC_pin_mode(pin_t pin) { + return (-1); +} + +int8_t get_pin_mode(pin_t pin) { + if (!VALID_PIN(pin)) return -1; + return 0; +} + +bool GET_PINMODE(pin_t pin) { + int8_t pin_mode = get_pin_mode(pin); + if (pin_mode == -1 || pin_mode == ADC_pin_mode(pin)) // found an invalid pin or active analog pin + return false; + + return (Gpio::getMode(pin) != 0); //input/output state +} + +bool GET_ARRAY_IS_DIGITAL(pin_t pin) { + return (!IS_ANALOG(pin) || get_pin_mode(pin) != ADC_pin_mode(pin)); +} diff --git a/src/HAL/LINUX/servo_private.h b/src/HAL/LINUX/servo_private.h new file mode 100644 index 0000000..bcc8d20 --- /dev/null +++ b/src/HAL/LINUX/servo_private.h @@ -0,0 +1,79 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * Based on "servo.h - Interrupt driven Servo library for Arduino using 16 bit timers - + * Version 2 Copyright (c) 2009 Michael Margolis. All right reserved. + * + * The only modification was to update/delete macros to match the LPC176x. + */ + +#include + +// Macros +//values in microseconds +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds + +#define MAX_SERVOS 4 + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + + +// Types + +typedef struct { + uint8_t nbr : 8 ; // a pin number from 0 to 254 (255 signals invalid pin) + uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t; + +typedef struct { + ServoPin_t Pin; + unsigned int pulse_width; // pulse width in microseconds +} ServoInfo_t; + +// Global variables + +extern uint8_t ServoCount; +extern ServoInfo_t servo_info[MAX_SERVOS]; diff --git a/src/HAL/LINUX/spi_pins.h b/src/HAL/LINUX/spi_pins.h new file mode 100644 index 0000000..33136ac --- /dev/null +++ b/src/HAL/LINUX/spi_pins.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/macros.h" +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(HAS_MARLINUI_U8GLIB, SDSUPPORT) && (LCD_PINS_D4 == SD_SCK_PIN || LCD_PINS_ENABLE == SD_MOSI_PIN || DOGLCD_SCK == SD_SCK_PIN || DOGLCD_MOSI == SD_MOSI_PIN) + #define LPC_SOFTWARE_SPI // If the SD card and LCD adapter share the same SPI pins, then software SPI is currently + // needed due to the speed and mode required for communicating with each device being different. + // This requirement can be removed if the SPI access to these devices is updated to use + // spiBeginTransaction. +#endif + +// Onboard SD +//#define SD_SCK_PIN P0_07 +//#define SD_MISO_PIN P0_08 +//#define SD_MOSI_PIN P0_09 +//#define SD_SS_PIN P0_06 + +// External SD +#ifndef SD_SCK_PIN + #define SD_SCK_PIN 50 +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN 51 +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN 52 +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN 53 +#endif +#ifndef SDSS + #define SDSS SD_SS_PIN +#endif diff --git a/src/HAL/LINUX/timers.cpp b/src/HAL/LINUX/timers.cpp new file mode 100644 index 0000000..66d80f2 --- /dev/null +++ b/src/HAL/LINUX/timers.cpp @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __PLAT_LINUX__ + +#include "hardware/Timer.h" + +#include "../../inc/MarlinConfig.h" + +/** + * Use POSIX signals to attempt to emulate Interrupts + * This has many limitations and is not fit for the purpose + */ + +HAL_STEP_TIMER_ISR(); +HAL_TEMP_TIMER_ISR(); + +Timer timers[2]; + +void HAL_timer_init() { + timers[0].init(0, STEPPER_TIMER_RATE, TIMER0_IRQHandler); + timers[1].init(1, TEMP_TIMER_RATE, TIMER1_IRQHandler); +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + timers[timer_num].start(frequency); +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + timers[timer_num].enable(); +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + timers[timer_num].disable(); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + return timers[timer_num].enabled(); +} + +void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + timers[timer_num].setCompare(compare); +} + +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + return timers[timer_num].getCompare(); +} + +hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + return timers[timer_num].getCount(); +} + +#endif // __PLAT_LINUX__ diff --git a/src/HAL/LINUX/timers.h b/src/HAL/LINUX/timers.h new file mode 100644 index 0000000..2d2a957 --- /dev/null +++ b/src/HAL/LINUX/timers.h @@ -0,0 +1,96 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL timers for Linux X86_64 + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define HAL_TIMER_RATE ((SystemCoreClock) / 4) // frequency of timers peripherals + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif + +#define TEMP_TIMER_RATE 1000000 +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void TIMER0_IRQHandler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void TIMER1_IRQHandler() +#endif + +// PWM timer +#define HAL_PWM_TIMER +#define HAL_PWM_TIMER_ISR() extern "C" void TIMER3_IRQHandler() +#define HAL_PWM_TIMER_IRQn + +void HAL_timer_init(); +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare); +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num); +hal_timer_t HAL_timer_get_count(const uint8_t timer_num); +FORCE_INLINE static void HAL_timer_restrain(const uint8_t timer_num, const uint16_t interval_ticks) { + const hal_timer_t mincmp = HAL_timer_get_count(timer_num) + interval_ticks; + if (HAL_timer_get_compare(timer_num) < mincmp) HAL_timer_set_compare(timer_num, mincmp); +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/LPC1768/HAL.cpp b/src/HAL/LPC1768/HAL.cpp new file mode 100644 index 0000000..9ff3a6b --- /dev/null +++ b/src/HAL/LPC1768/HAL.cpp @@ -0,0 +1,124 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" +#include "../shared/Delay.h" +#include "../../../gcode/parser.h" + +DefaultSerial1 USBSerial(false, UsbSerial); + +uint32_t MarlinHAL::adc_result = 0; + +// U8glib required functions +extern "C" { + void u8g_xMicroDelay(uint16_t val) { DELAY_US(val); } + void u8g_MicroDelay() { u8g_xMicroDelay(1); } + void u8g_10MicroDelay() { u8g_xMicroDelay(10); } + void u8g_Delay(uint16_t val) { delay(val); } +} + +// return free heap space +int freeMemory() { + char stack_end; + void *heap_start = malloc(sizeof(uint32_t)); + if (heap_start == 0) return 0; + + uint32_t result = (uint32_t)&stack_end - (uint32_t)heap_start; + free(heap_start); + return result; +} + +void MarlinHAL::reboot() { NVIC_SystemReset(); } + +uint8_t MarlinHAL::get_reset_source() { + #if ENABLED(USE_WATCHDOG) + if (watchdog_timed_out()) return RST_WATCHDOG; + #endif + return RST_POWER_ON; +} + +void MarlinHAL::clear_reset_source() { watchdog_clear_timeout_flag(); } + +void flashFirmware(const int16_t) { + delay(500); // Give OS time to disconnect + USB_Connect(false); // USB clear connection + delay(1000); // Give OS time to notice + hal.reboot(); +} + +#if ENABLED(USE_WATCHDOG) + + #include + + #define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout + + void MarlinHAL::watchdog_init() { + #if ENABLED(WATCHDOG_RESET_MANUAL) + // We enable the watchdog timer, but only for the interrupt. + + // Configure WDT to only trigger an interrupt + // Disable WDT interrupt (just in case, to avoid triggering it!) + NVIC_DisableIRQ(WDT_IRQn); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); + + // Configure WDT to only trigger an interrupt + // Initialize WDT with the given parameters + WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_INT_ONLY); + + // Configure and enable WDT interrupt. + NVIC_ClearPendingIRQ(WDT_IRQn); + NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups + NVIC_EnableIRQ(WDT_IRQn); + #else + WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_RESET); + #endif + WDT_Start(WDT_TIMEOUT_US); + } + + void MarlinHAL::watchdog_refresh() { + WDT_Feed(); + #if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED) + TOGGLE(LED_PIN); // heartbeat indicator + #endif + } + + // Timeout state + bool MarlinHAL::watchdog_timed_out() { return TEST(WDT_ReadTimeOutFlag(), 0); } + void MarlinHAL::watchdog_clear_timeout_flag() { WDT_ClrTimeOutFlag(); } + +#endif // USE_WATCHDOG + +// For M42/M43, scan command line for pin code +// return index into pin map array if found and the pin is valid. +// return dval if not found or not a valid pin. +int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) { + const uint16_t val = (uint16_t)parser.intval(code, -1), port = val / 100, pin = val % 100; + const int16_t ind = (port < ((NUM_DIGITAL_PINS) >> 5) && pin < 32) ? ((port << 5) | pin) : -2; + return ind > -1 ? ind : dval; +} + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/HAL.h b/src/HAL/LPC1768/HAL.h new file mode 100644 index 0000000..b0eeb98 --- /dev/null +++ b/src/HAL/LPC1768/HAL.h @@ -0,0 +1,267 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL_LPC1768/HAL.h + * Hardware Abstraction Layer for NXP LPC1768 + */ + +#define CPU_32_BIT + +#include +#include +#include + +extern "C" volatile uint32_t _millis; + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "MarlinSerial.h" + +#include +#include +#include + +// ------------------------ +// Serial ports +// ------------------------ + +typedef ForwardSerial1Class< decltype(UsbSerial) > DefaultSerial1; +extern DefaultSerial1 USBSerial; + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if SERIAL_PORT == -1 + #define MYSERIAL1 USBSerial +#elif WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." +#endif + +#ifdef SERIAL_PORT_2 + #if SERIAL_PORT_2 == -1 + #define MYSERIAL2 USBSerial + #elif WITHIN(SERIAL_PORT_2, 0, 3) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 + #define MYSERIAL3 USBSerial + #elif WITHIN(SERIAL_PORT_3, 0, 3) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #else + #error "SERIAL_PORT_3 must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if MMU2_SERIAL_PORT == -1 + #define MMU2_SERIAL USBSerial + #elif WITHIN(MMU2_SERIAL_PORT, 0, 3) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #error "MMU2_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if LCD_SERIAL_PORT == -1 + #define LCD_SERIAL USBSerial + #elif WITHIN(LCD_SERIAL_PORT, 0, 3) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #error "LCD_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + #if HAS_DGUS_LCD + #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.available() + #endif +#endif + +// +// Interrupts +// + +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() + +// +// ADC +// + +#define ADC_MEDIAN_FILTER_SIZE (23) // Higher values increase step delay (phase shift), + // (ADC_MEDIAN_FILTER_SIZE + 1) / 2 sample step delay (12 samples @ 500Hz: 24ms phase shift) + // Memory usage per ADC channel (bytes): (6 * ADC_MEDIAN_FILTER_SIZE) + 16 + // 8 * ((6 * 23) + 16 ) = 1232 Bytes for 8 channels + +#define ADC_LOWPASS_K_VALUE (2) // Higher values increase rise time + // Rise time sample delays for 100% signal convergence on full range step + // (1 : 13, 2 : 32, 3 : 67, 4 : 139, 5 : 281, 6 : 565, 7 : 1135, 8 : 2273) + // K = 6, 565 samples, 500Hz sample rate, 1.13s convergence on full range step + // Memory usage per ADC channel (bytes): 4 (32 Bytes for 8 channels) + +#define HAL_ADC_VREF 3.3 // ADC voltage reference + +#define HAL_ADC_RESOLUTION 12 // 15 bit maximum, raw temperature is stored as int16_t +#define HAL_ADC_FILTERED // Disable oversampling done in Marlin as ADC values already filtered in HAL + +// +// Pin Mapping for M42, M43, M226 +// + +// Test whether the pin is valid +constexpr bool VALID_PIN(const pin_t pin) { + return LPC176x::pin_is_valid(pin); +} + +// Get the analog index for a digital pin +constexpr int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t pin) { + return (LPC176x::pin_is_valid(pin) && LPC176x::pin_has_adc(pin)) ? pin : -1; +} + +// Return the index of a pin number +constexpr int16_t GET_PIN_MAP_INDEX(const pin_t pin) { + return LPC176x::pin_index(pin); +} + +// Get the pin number at the given index +constexpr pin_t GET_PIN_MAP_PIN(const int16_t index) { + return LPC176x::pin_index(index); +} + +// Parse a G-code word into a pin index +int16_t PARSED_PIN_INDEX(const char code, const int16_t dval); +// P0.6 thru P0.9 are for the onboard SD card +#define HAL_SENSITIVE_PINS P0_06, P0_07, P0_08, P0_09, + +// ------------------------ +// Defines +// ------------------------ + +#define PLATFORM_M997_SUPPORT +void flashFirmware(const int16_t); + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment + +// Default graphical display delays +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + static bool watchdog_timed_out() IF_DISABLED(USE_WATCHDOG, { return false; }); + static void watchdog_clear_timeout_flag() IF_DISABLED(USE_WATCHDOG, {}); + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + using FilteredADC = LPC176x::ADC; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { + FilteredADC::enable_channel(pin); + } + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static uint32_t adc_result; + static void adc_start(const pin_t pin) { + adc_result = FilteredADC::read(pin) >> (16 - HAL_ADC_RESOLUTION); // returns 16bit value, reduce to required bits + } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return uint16_t(adc_result); } + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Set the frequency of the timer corresponding to the provided pin + * All Hardware PWM pins will run at the same frequency and + * All Software PWM pins will run at the same frequency + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); +}; diff --git a/src/HAL/LPC1768/HAL_SPI.cpp b/src/HAL/LPC1768/HAL_SPI.cpp new file mode 100644 index 0000000..29f9b43 --- /dev/null +++ b/src/HAL/LPC1768/HAL_SPI.cpp @@ -0,0 +1,406 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Software SPI functions originally from Arduino Sd2Card Library + * Copyright (c) 2009 by William Greiman + */ + +/** + * For TARGET_LPC1768 + */ + +/** + * Hardware SPI and Software SPI implementations are included in this file. + * The hardware SPI runs faster and has higher throughput but is not compatible + * with some LCD interfaces/adapters. + * + * Control of the slave select pin(s) is handled by the calling routines. + * + * Some of the LCD interfaces/adapters result in the LCD SPI and the SD card + * SPI sharing pins. The SCK, MOSI & MISO pins can NOT be set/cleared with + * WRITE nor digitalWrite when the hardware SPI module within the LPC17xx is + * active. If any of these pins are shared then the software SPI must be used. + * + * A more sophisticated hardware SPI can be found at the following link. + * This implementation has not been fully debugged. + * https://github.com/MarlinFirmware/Marlin/tree/071c7a78f27078fd4aee9a3ef365fcf5e143531e + */ + +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" +#include + +// Hardware SPI and SPIClass +#include +#include + +#include "../shared/HAL_SPI.h" + +// ------------------------ +// Public functions +// ------------------------ +#if ENABLED(LPC_SOFTWARE_SPI) + + // Software SPI + + #include + + static uint8_t SPI_speed = SPI_FULL_SPEED; + + static uint8_t spiTransfer(uint8_t b) { + return swSpiTransfer(b, SPI_speed, SD_SCK_PIN, SD_MISO_PIN, SD_MOSI_PIN); + } + + void spiBegin() { + swSpiBegin(SD_SCK_PIN, SD_MISO_PIN, SD_MOSI_PIN); + } + + void spiInit(uint8_t spiRate) { + SPI_speed = swSpiInit(spiRate, SD_SCK_PIN, SD_MOSI_PIN); + } + + uint8_t spiRec() { return spiTransfer(0xFF); } + + void spiRead(uint8_t*buf, uint16_t nbyte) { + for (int i = 0; i < nbyte; i++) + buf[i] = spiTransfer(0xFF); + } + + void spiSend(uint8_t b) { (void)spiTransfer(b); } + + void spiSend(const uint8_t *buf, size_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) + (void)spiTransfer(buf[i]); + } + + void spiSendBlock(uint8_t token, const uint8_t *buf) { + (void)spiTransfer(token); + for (uint16_t i = 0; i < 512; i++) + (void)spiTransfer(buf[i]); + } + +#else + + #ifdef SD_SPI_SPEED + #define INIT_SPI_SPEED SD_SPI_SPEED + #else + #define INIT_SPI_SPEED SPI_FULL_SPEED + #endif + + void spiBegin() { spiInit(INIT_SPI_SPEED); } // Set up SCK, MOSI & MISO pins for SSP0 + + void spiInit(uint8_t spiRate) { + #if SD_MISO_PIN == BOARD_SPI1_MISO_PIN + SPI.setModule(1); + #elif SD_MISO_PIN == BOARD_SPI2_MISO_PIN + SPI.setModule(2); + #endif + SPI.setDataSize(DATA_SIZE_8BIT); + SPI.setDataMode(SPI_MODE0); + + SPI.setClock(SPISettings::spiRate2Clock(spiRate)); + SPI.begin(); + } + + static uint8_t doio(uint8_t b) { + return SPI.transfer(b & 0x00FF) & 0x00FF; + } + + void spiSend(uint8_t b) { doio(b); } + + void spiSend(const uint8_t *buf, size_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) doio(buf[i]); + } + + void spiSend(uint32_t chan, byte b) {} + + void spiSend(uint32_t chan, const uint8_t *buf, size_t nbyte) {} + + // Read single byte from SPI + uint8_t spiRec() { return doio(0xFF); } + + uint8_t spiRec(uint32_t chan) { return 0; } + + // Read from SPI into buffer + void spiRead(uint8_t *buf, uint16_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) buf[i] = doio(0xFF); + } + + uint8_t spiTransfer(uint8_t b) { return doio(b); } + + // Write from buffer to SPI + void spiSendBlock(uint8_t token, const uint8_t *buf) { + (void)spiTransfer(token); + for (uint16_t i = 0; i < 512; i++) + (void)spiTransfer(buf[i]); + } + + // Begin SPI transaction, set clock, bit order, data mode + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + // TODO: Implement this method + } + +#endif // LPC_SOFTWARE_SPI + +/** + * @brief Wait until TXE (tx empty) flag is set and BSY (busy) flag unset. + */ +static inline void waitSpiTxEnd(LPC_SSP_TypeDef *spi_d) { + while (SSP_GetStatus(spi_d, SSP_STAT_TXFIFO_EMPTY) == RESET) { /* nada */ } // wait until TXE=1 + while (SSP_GetStatus(spi_d, SSP_STAT_BUSY) == SET) { /* nada */ } // wait until BSY=0 +} + +// Retain the pin init state of the SPI, to avoid init more than once, +// even if more instances of SPIClass exist +static bool spiInitialised[BOARD_NR_SPI] = { false }; + +SPIClass::SPIClass(uint8_t device) { + // Init things specific to each SPI device + // clock divider setup is a bit of hack, and needs to be improved at a later date. + + #if BOARD_NR_SPI >= 1 + _settings[0].spi_d = LPC_SSP0; + _settings[0].dataMode = SPI_MODE0; + _settings[0].dataSize = DATA_SIZE_8BIT; + _settings[0].clock = SPI_CLOCK_MAX; + //_settings[0].clockDivider = determine_baud_rate(_settings[0].spi_d, _settings[0].clock); + #endif + + #if BOARD_NR_SPI >= 2 + _settings[1].spi_d = LPC_SSP1; + _settings[1].dataMode = SPI_MODE0; + _settings[1].dataSize = DATA_SIZE_8BIT; + _settings[1].clock = SPI_CLOCK_MAX; + //_settings[1].clockDivider = determine_baud_rate(_settings[1].spi_d, _settings[1].clock); + #endif + + setModule(device); + + // Init the GPDMA controller + // TODO: call once in the constructor? or each time? + GPDMA_Init(); +} + +SPIClass::SPIClass(pin_t mosi, pin_t miso, pin_t sclk, pin_t ssel) { + #if BOARD_NR_SPI >= 1 + if (mosi == BOARD_SPI1_MOSI_PIN) SPIClass(1); + #endif + #if BOARD_NR_SPI >= 2 + if (mosi == BOARD_SPI2_MOSI_PIN) SPIClass(2); + #endif +} + +void SPIClass::begin() { + // Init the SPI pins in the first begin call + if ((_currentSetting->spi_d == LPC_SSP0 && spiInitialised[0] == false) || + (_currentSetting->spi_d == LPC_SSP1 && spiInitialised[1] == false)) { + pin_t sck, miso, mosi; + if (_currentSetting->spi_d == LPC_SSP0) { + sck = BOARD_SPI1_SCK_PIN; + miso = BOARD_SPI1_MISO_PIN; + mosi = BOARD_SPI1_MOSI_PIN; + spiInitialised[0] = true; + } + else if (_currentSetting->spi_d == LPC_SSP1) { + sck = BOARD_SPI2_SCK_PIN; + miso = BOARD_SPI2_MISO_PIN; + mosi = BOARD_SPI2_MOSI_PIN; + spiInitialised[1] = true; + } + PINSEL_CFG_Type PinCfg; // data structure to hold init values + PinCfg.Funcnum = 2; + PinCfg.OpenDrain = 0; + PinCfg.Pinmode = 0; + PinCfg.Pinnum = LPC176x::pin_bit(sck); + PinCfg.Portnum = LPC176x::pin_port(sck); + PINSEL_ConfigPin(&PinCfg); + SET_OUTPUT(sck); + + PinCfg.Pinnum = LPC176x::pin_bit(miso); + PinCfg.Portnum = LPC176x::pin_port(miso); + PINSEL_ConfigPin(&PinCfg); + SET_INPUT(miso); + + PinCfg.Pinnum = LPC176x::pin_bit(mosi); + PinCfg.Portnum = LPC176x::pin_port(mosi); + PINSEL_ConfigPin(&PinCfg); + SET_OUTPUT(mosi); + } + + updateSettings(); + SSP_Cmd(_currentSetting->spi_d, ENABLE); // start SSP running +} + +void SPIClass::beginTransaction(const SPISettings &cfg) { + setBitOrder(cfg.bitOrder); + setDataMode(cfg.dataMode); + setDataSize(cfg.dataSize); + //setClockDivider(determine_baud_rate(_currentSetting->spi_d, settings.clock)); + begin(); +} + +uint8_t SPIClass::transfer(const uint16_t b) { + // Send and receive a single byte + SSP_ReceiveData(_currentSetting->spi_d); // read any previous data + SSP_SendData(_currentSetting->spi_d, b); + waitSpiTxEnd(_currentSetting->spi_d); // wait for it to finish + return SSP_ReceiveData(_currentSetting->spi_d); +} + +uint16_t SPIClass::transfer16(const uint16_t data) { + return (transfer((data >> 8) & 0xFF) << 8) | (transfer(data & 0xFF) & 0xFF); +} + +void SPIClass::end() { + // Neither is needed for Marlin + //SSP_Cmd(_currentSetting->spi_d, DISABLE); + //SSP_DeInit(_currentSetting->spi_d); +} + +void SPIClass::send(uint8_t data) { + SSP_SendData(_currentSetting->spi_d, data); +} + +void SPIClass::dmaSend(void *buf, uint16_t length, bool minc) { + //TODO: LPC dma can only write 0xFFF bytes at once. + GPDMA_Channel_CFG_Type GPDMACfg; + + /* Configure GPDMA channel 0 -------------------------------------------------------------*/ + /* DMA Channel 0 */ + GPDMACfg.ChannelNum = 0; + // Source memory + GPDMACfg.SrcMemAddr = (uint32_t)buf; + // Destination memory - Not used + GPDMACfg.DstMemAddr = 0; + // Transfer size + GPDMACfg.TransferSize = length; + // Transfer width + GPDMACfg.TransferWidth = (_currentSetting->dataSize == DATA_SIZE_16BIT) ? GPDMA_WIDTH_HALFWORD : GPDMA_WIDTH_BYTE; + // Transfer type + GPDMACfg.TransferType = GPDMA_TRANSFERTYPE_M2P; + // Source connection - unused + GPDMACfg.SrcConn = 0; + // Destination connection + GPDMACfg.DstConn = (_currentSetting->spi_d == LPC_SSP0) ? GPDMA_CONN_SSP0_Tx : GPDMA_CONN_SSP1_Tx; + + GPDMACfg.DMALLI = 0; + + // Enable dma on SPI + SSP_DMACmd(_currentSetting->spi_d, SSP_DMA_TX, ENABLE); + + // Only increase memory if minc is true + GPDMACfg.MemoryIncrease = (minc ? GPDMA_DMACCxControl_SI : 0); + + // Setup channel with given parameter + GPDMA_Setup(&GPDMACfg); + + // Enable DMA + GPDMA_ChannelCmd(0, ENABLE); + + // Wait for data transfer + while (!GPDMA_IntGetStatus(GPDMA_STAT_RAWINTTC, 0) && !GPDMA_IntGetStatus(GPDMA_STAT_RAWINTERR, 0)) { } + + // Clear err and int + GPDMA_ClearIntPending (GPDMA_STATCLR_INTTC, 0); + GPDMA_ClearIntPending (GPDMA_STATCLR_INTERR, 0); + + // Disable DMA + GPDMA_ChannelCmd(0, DISABLE); + + waitSpiTxEnd(_currentSetting->spi_d); + + SSP_DMACmd(_currentSetting->spi_d, SSP_DMA_TX, DISABLE); +} + +uint16_t SPIClass::read() { + return SSP_ReceiveData(_currentSetting->spi_d); +} + +void SPIClass::read(uint8_t *buf, uint32_t len) { + for (uint16_t i = 0; i < len; i++) buf[i] = transfer(0xFF); +} + +void SPIClass::setClock(uint32_t clock) { _currentSetting->clock = clock; } + +void SPIClass::setModule(uint8_t device) { _currentSetting = &_settings[device - 1]; } // SPI channels are called 1, 2, and 3 but the array is zero-indexed + +void SPIClass::setBitOrder(uint8_t bitOrder) { _currentSetting->bitOrder = bitOrder; } + +void SPIClass::setDataMode(uint8_t dataMode) { _currentSetting->dataMode = dataMode; } + +void SPIClass::setDataSize(uint32_t dataSize) { _currentSetting->dataSize = dataSize; } + +/** + * Set up/tear down + */ +void SPIClass::updateSettings() { + //SSP_DeInit(_currentSetting->spi_d); //todo: need force de init?! + + // Divide PCLK by 2 for SSP0 + //CLKPWR_SetPCLKDiv(_currentSetting->spi_d == LPC_SSP0 ? CLKPWR_PCLKSEL_SSP0 : CLKPWR_PCLKSEL_SSP1, CLKPWR_PCLKSEL_CCLK_DIV_2); + + SSP_CFG_Type HW_SPI_init; // data structure to hold init values + SSP_ConfigStructInit(&HW_SPI_init); // set values for SPI mode + HW_SPI_init.ClockRate = _currentSetting->clock; + HW_SPI_init.Databit = _currentSetting->dataSize; + + /** + * SPI Mode CPOL CPHA Shift SCK-edge Capture SCK-edge + * 0 0 0 Falling Rising + * 1 0 1 Rising Falling + * 2 1 0 Rising Falling + * 3 1 1 Falling Rising + */ + switch (_currentSetting->dataMode) { + case SPI_MODE0: + HW_SPI_init.CPHA = SSP_CPHA_FIRST; + HW_SPI_init.CPOL = SSP_CPOL_HI; + break; + case SPI_MODE1: + HW_SPI_init.CPHA = SSP_CPHA_SECOND; + HW_SPI_init.CPOL = SSP_CPOL_HI; + break; + case SPI_MODE2: + HW_SPI_init.CPHA = SSP_CPHA_FIRST; + HW_SPI_init.CPOL = SSP_CPOL_LO; + break; + case SPI_MODE3: + HW_SPI_init.CPHA = SSP_CPHA_SECOND; + HW_SPI_init.CPOL = SSP_CPOL_LO; + break; + default: + break; + } + + // TODO: handle bitOrder + SSP_Init(_currentSetting->spi_d, &HW_SPI_init); // puts the values into the proper bits in the SSP0 registers +} + +#if SD_MISO_PIN == BOARD_SPI1_MISO_PIN + SPIClass SPI(1); +#elif SD_MISO_PIN == BOARD_SPI2_MISO_PIN + SPIClass SPI(2); +#endif + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/MarlinSPI.h b/src/HAL/LPC1768/MarlinSPI.h new file mode 100644 index 0000000..fab245f --- /dev/null +++ b/src/HAL/LPC1768/MarlinSPI.h @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +/** + * Marlin currently requires 3 SPI classes: + * + * SPIClass: + * This class is normally provided by frameworks and has a semi-default interface. + * This is needed because some libraries reference it globally. + * + * SPISettings: + * Container for SPI configs for SPIClass. As above, libraries may reference it globally. + * + * These two classes are often provided by frameworks so we cannot extend them to add + * useful methods for Marlin. + * + * MarlinSPI: + * Provides the default SPIClass interface plus some Marlin goodies such as a simplified + * interface for SPI DMA transfer. + * + */ + +using MarlinSPI = SPIClass; diff --git a/src/HAL/LPC1768/MarlinSerial.cpp b/src/HAL/LPC1768/MarlinSerial.cpp new file mode 100644 index 0000000..f2aecf5 --- /dev/null +++ b/src/HAL/LPC1768/MarlinSerial.cpp @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "MarlinSerial.h" + +#include "../../inc/MarlinConfig.h" + +#if USING_HW_SERIAL0 + MarlinSerial _MSerial0(LPC_UART0); + MSerialT MSerial0(true, _MSerial0); + extern "C" void UART0_IRQHandler() { _MSerial0.IRQHandler(); } +#endif +#if USING_HW_SERIAL1 + MarlinSerial _MSerial1((LPC_UART_TypeDef *) LPC_UART1); + MSerialT MSerial1(true, _MSerial1); + extern "C" void UART1_IRQHandler() { _MSerial1.IRQHandler(); } +#endif +#if USING_HW_SERIAL2 + MarlinSerial _MSerial2(LPC_UART2); + MSerialT MSerial2(true, _MSerial2); + extern "C" void UART2_IRQHandler() { _MSerial2.IRQHandler(); } +#endif +#if USING_HW_SERIAL3 + MarlinSerial _MSerial3(LPC_UART3); + MSerialT MSerial3(true, _MSerial3); + extern "C" void UART3_IRQHandler() { _MSerial3.IRQHandler(); } +#endif + +#if ENABLED(EMERGENCY_PARSER) + + bool MarlinSerial::recv_callback(const char c) { + // Need to figure out which serial port we are and react in consequence (Marlin does not have CONTAINER_OF macro) + if (false) {} + #if USING_HW_SERIAL0 + else if (this == &_MSerial0) emergency_parser.update(MSerial0.emergency_state, c); + #endif + #if USING_HW_SERIAL1 + else if (this == &_MSerial1) emergency_parser.update(MSerial1.emergency_state, c); + #endif + #if USING_HW_SERIAL2 + else if (this == &_MSerial2) emergency_parser.update(MSerial2.emergency_state, c); + #endif + #if USING_HW_SERIAL3 + else if (this == &_MSerial3) emergency_parser.update(MSerial3.emergency_state, c); + #endif + return true; + } + +#endif + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/MarlinSerial.h b/src/HAL/LPC1768/MarlinSerial.h new file mode 100644 index 0000000..3e6848a --- /dev/null +++ b/src/HAL/LPC1768/MarlinSerial.h @@ -0,0 +1,69 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include + +#include "../../inc/MarlinConfigPre.h" +#if ENABLED(EMERGENCY_PARSER) + #include "../../feature/e_parser.h" +#endif +#include "../../core/serial_hook.h" + +#ifndef SERIAL_PORT + #define SERIAL_PORT 0 +#endif +#ifndef RX_BUFFER_SIZE + #define RX_BUFFER_SIZE 128 +#endif +#ifndef TX_BUFFER_SIZE + #define TX_BUFFER_SIZE 32 +#endif + +class MarlinSerial : public HardwareSerial { +public: + MarlinSerial(LPC_UART_TypeDef *UARTx) : HardwareSerial(UARTx) { } + + void end() {} + + uint8_t availableForWrite(void) { /* flushTX(); */ return TX_BUFFER_SIZE; } + + #if ENABLED(EMERGENCY_PARSER) + bool recv_callback(const char c) override; + #endif +}; + +// On LPC176x framework, HardwareSerial does not implement the same interface as Arduino's Serial, so overloads +// of 'available' and 'read' method are not used in this multiple inheritance scenario. +// Instead, use a ForwardSerial here that adapts the interface. +typedef ForwardSerial1Class MSerialT; +extern MSerialT MSerial0; +extern MSerialT MSerial1; +extern MSerialT MSerial2; +extern MSerialT MSerial3; + +// Consequently, we can't use a RuntimeSerial either. The workaround would be to use +// a RuntimeSerial> type here. Ignore for now until it's actually required. +#if ENABLED(SERIAL_RUNTIME_HOOK) + #error "SERIAL_RUNTIME_HOOK is not yet supported for LPC176x." +#endif diff --git a/src/HAL/LPC1768/MinSerial.cpp b/src/HAL/LPC1768/MinSerial.cpp new file mode 100644 index 0000000..7a1c038 --- /dev/null +++ b/src/HAL/LPC1768/MinSerial.cpp @@ -0,0 +1,51 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#if ENABLED(POSTMORTEM_DEBUGGING) + +#include "../shared/MinSerial.h" +#include + +static void TX(char c) { _DBC(c); } +void install_min_serial() { HAL_min_serial_out = &TX; } + +#if DISABLED(DYNAMIC_VECTORTABLE) +extern "C" { + __attribute__((naked)) void JumpHandler_ASM() { + __asm__ __volatile__ ( + "b CommonHandler_ASM\n" + ); + } + void __attribute__((naked, alias("JumpHandler_ASM"))) HardFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) BusFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) UsageFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) MemManage_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"))) NMI_Handler(); +} +#endif + +#endif // POSTMORTEM_DEBUGGING +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/Servo.h b/src/HAL/LPC1768/Servo.h new file mode 100644 index 0000000..f02f503 --- /dev/null +++ b/src/HAL/LPC1768/Servo.h @@ -0,0 +1,69 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ +#pragma once + +/** + * Based on "servo.h - Interrupt driven Servo library for Arduino using 16 bit timers - + * Version 2 Copyright (c) 2009 Michael Margolis. All right reserved. + * + * The only modification was to update/delete macros to match the LPC176x. + */ + +#include + +class libServo: public Servo { + public: + void move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + + if (attach(servo_info[servoIndex].Pin.nbr) >= 0) { // try to reattach + write(value); + safe_delay(servo_delay[servoIndex]); // delay to allow servo to reach position + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } + + } +}; + +class libServo; +typedef libServo hal_servo_t; diff --git a/src/HAL/LPC1768/eeprom_flash.cpp b/src/HAL/LPC1768/eeprom_flash.cpp new file mode 100644 index 0000000..38d2705 --- /dev/null +++ b/src/HAL/LPC1768/eeprom_flash.cpp @@ -0,0 +1,131 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +/** + * Emulate EEPROM storage using Flash Memory + * + * Use a single 32K flash sector to store EEPROM data. To reduce the + * number of erase operations a simple "leveling" scheme is used that + * maintains a number of EEPROM "slots" within the larger flash sector. + * Each slot is used in turn and the entire sector is only erased when all + * slots have been used. + * + * A simple RAM image is used to hold the EEPROM data during I/O operations + * and this is flushed to the next available slot when an update is complete. + * If RAM usage becomes an issue we could store this image in one of the two + * 16Kb I/O buffers (intended to hold DMA USB and Ethernet data, but currently + * unused). + */ +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FLASH_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" + +extern "C" { + #include +} + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif + +#define SECTOR_START(sector) ((sector < 16) ? (sector << 12) : ((sector - 14) << 15)) +#define EEPROM_SECTOR 29 +#define SECTOR_SIZE 32768 +#define EEPROM_SLOTS ((SECTOR_SIZE)/(MARLIN_EEPROM_SIZE)) +#define EEPROM_ERASE 0xFF +#define SLOT_ADDRESS(sector, slot) (((uint8_t *)SECTOR_START(sector)) + slot * (MARLIN_EEPROM_SIZE)) + +static uint8_t ram_eeprom[MARLIN_EEPROM_SIZE] __attribute__((aligned(4))) = {0}; +static bool eeprom_dirty = false; +static int current_slot = 0; + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { + uint32_t first_nblank_loc, first_nblank_val; + IAP_STATUS_CODE status; + + // discover which slot we are currently using. + __disable_irq(); + status = BlankCheckSector(EEPROM_SECTOR, EEPROM_SECTOR, &first_nblank_loc, &first_nblank_val); + __enable_irq(); + + if (status == CMD_SUCCESS) { + // sector is blank so nothing stored yet + for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = EEPROM_ERASE; + current_slot = EEPROM_SLOTS; + } + else { + // current slot is the first non blank one + current_slot = first_nblank_loc / (MARLIN_EEPROM_SIZE); + uint8_t *eeprom_data = SLOT_ADDRESS(EEPROM_SECTOR, current_slot); + // load current settings + for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = eeprom_data[i]; + } + eeprom_dirty = false; + + return true; +} + +bool PersistentStore::access_finish() { + if (eeprom_dirty) { + IAP_STATUS_CODE status; + if (--current_slot < 0) { + // all slots have been used, erase everything and start again + __disable_irq(); + status = EraseSector(EEPROM_SECTOR, EEPROM_SECTOR); + __enable_irq(); + + current_slot = EEPROM_SLOTS - 1; + } + + __disable_irq(); + status = CopyRAM2Flash(SLOT_ADDRESS(EEPROM_SECTOR, current_slot), ram_eeprom, IAP_WRITE_4096); + __enable_irq(); + + if (status != CMD_SUCCESS) return false; + eeprom_dirty = false; + } + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + for (size_t i = 0; i < size; i++) ram_eeprom[pos + i] = value[i]; + eeprom_dirty = true; + crc16(crc, value, size); + pos += size; + return false; // return true for any error +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + const uint8_t * const buff = writing ? &value[0] : &ram_eeprom[pos]; + if (writing) for (size_t i = 0; i < size; i++) value[i] = ram_eeprom[pos + i]; + crc16(crc, buff, size); + pos += size; + return false; // return true for any error +} + +#endif // FLASH_EEPROM_EMULATION +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/eeprom_sdcard.cpp b/src/HAL/LPC1768/eeprom_sdcard.cpp new file mode 100644 index 0000000..1991d79 --- /dev/null +++ b/src/HAL/LPC1768/eeprom_sdcard.cpp @@ -0,0 +1,186 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Implementation of EEPROM settings in SD Card + */ + +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SDCARD_EEPROM_EMULATION) + +//#define DEBUG_SD_EEPROM_EMULATION + +#include "../shared/eeprom_api.h" + +#include +#include + +extern uint32_t MSC_Aquire_Lock(); +extern uint32_t MSC_Release_Lock(); + +FATFS fat_fs; +FIL eeprom_file; +bool eeprom_file_open = false; + +#define EEPROM_FILENAME "eeprom.dat" +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(0x1000) // 4KiB of Emulated EEPROM +#endif + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { + const char eeprom_erase_value = 0xFF; + MSC_Aquire_Lock(); + if (f_mount(&fat_fs, "", 1)) { + MSC_Release_Lock(); + return false; + } + FRESULT res = f_open(&eeprom_file, EEPROM_FILENAME, FA_OPEN_ALWAYS | FA_WRITE | FA_READ); + if (res) MSC_Release_Lock(); + + if (res == FR_OK) { + UINT bytes_written; + FSIZE_t file_size = f_size(&eeprom_file); + f_lseek(&eeprom_file, file_size); + while (file_size < capacity() && res == FR_OK) { + res = f_write(&eeprom_file, &eeprom_erase_value, 1, &bytes_written); + file_size++; + } + } + if (res == FR_OK) { + f_lseek(&eeprom_file, 0); + f_sync(&eeprom_file); + eeprom_file_open = true; + } + return res == FR_OK; +} + +bool PersistentStore::access_finish() { + f_close(&eeprom_file); + f_unmount(""); + MSC_Release_Lock(); + eeprom_file_open = false; + return true; +} + +// This extra chit-chat goes away soon, but is helpful for now +// to see errors that are happening in read_data / write_data +static void debug_rw(const bool write, int &pos, const uint8_t *value, const size_t size, const FRESULT s, const size_t total=0) { + #if ENABLED(DEBUG_SD_EEPROM_EMULATION) + FSTR_P const rw_str = write ? F("write") : F("read"); + SERIAL_CHAR(' '); + SERIAL_ECHOF(rw_str); + SERIAL_ECHOLNPGM("_data(", pos, ",", *value, ",", size, ", ...)"); + if (total) { + SERIAL_ECHOPGM(" f_"); + SERIAL_ECHOF(rw_str); + SERIAL_ECHOPGM("()=", s, "\n size=", size, "\n bytes_"); + SERIAL_ECHOLNF(write ? F("written=") : F("read="), total); + } + else + SERIAL_ECHOLNPGM(" f_lseek()=", s); + #endif +} + +// File function return codes for type FRESULT. This goes away soon, but +// is helpful right now to see any errors in read_data and write_data. +// +// typedef enum { +// FR_OK = 0, /* (0) Succeeded */ +// FR_DISK_ERR, /* (1) A hard error occurred in the low level disk I/O layer */ +// FR_INT_ERR, /* (2) Assertion failed */ +// FR_NOT_READY, /* (3) The physical drive cannot work */ +// FR_NO_FILE, /* (4) Could not find the file */ +// FR_NO_PATH, /* (5) Could not find the path */ +// FR_INVALID_NAME, /* (6) The path name format is invalid */ +// FR_DENIED, /* (7) Access denied due to prohibited access or directory full */ +// FR_EXIST, /* (8) Access denied due to prohibited access */ +// FR_INVALID_OBJECT, /* (9) The file/directory object is invalid */ +// FR_WRITE_PROTECTED, /* (10) The physical drive is write protected */ +// FR_INVALID_DRIVE, /* (11) The logical drive number is invalid */ +// FR_NOT_ENABLED, /* (12) The volume has no work area */ +// FR_NO_FILESYSTEM, /* (13) There is no valid FAT volume */ +// FR_MKFS_ABORTED, /* (14) The f_mkfs() aborted due to any problem */ +// FR_TIMEOUT, /* (15) Could not get a grant to access the volume within defined period */ +// FR_LOCKED, /* (16) The operation is rejected according to the file sharing policy */ +// FR_NOT_ENOUGH_CORE, /* (17) LFN working buffer could not be allocated */ +// FR_TOO_MANY_OPEN_FILES, /* (18) Number of open files > FF_FS_LOCK */ +// FR_INVALID_PARAMETER /* (19) Given parameter is invalid */ +// } FRESULT; + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + if (!eeprom_file_open) return true; + FRESULT s; + UINT bytes_written = 0; + + s = f_lseek(&eeprom_file, pos); + if (s) { + debug_rw(true, pos, value, size, s); + return s; + } + + s = f_write(&eeprom_file, (void*)value, size, &bytes_written); + if (s) { + debug_rw(true, pos, value, size, s, bytes_written); + return s; + } + crc16(crc, value, size); + pos += size; + return bytes_written != size; // return true for any error +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uint16_t *crc, const bool writing/*=true*/) { + if (!eeprom_file_open) return true; + UINT bytes_read = 0; + FRESULT s; + s = f_lseek(&eeprom_file, pos); + + if (s) { + debug_rw(false, pos, value, size, s); + return true; + } + + if (writing) { + s = f_read(&eeprom_file, (void*)value, size, &bytes_read); + crc16(crc, value, size); + } + else { + uint8_t temp[size]; + s = f_read(&eeprom_file, (void*)temp, size, &bytes_read); + crc16(crc, temp, size); + } + + if (s) { + debug_rw(false, pos, value, size, s, bytes_read); + return true; + } + + pos += size; + return bytes_read != size; // return true for any error +} + +#endif // SDCARD_EEPROM_EMULATION +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/eeprom_wired.cpp b/src/HAL/LPC1768/eeprom_wired.cpp new file mode 100644 index 0000000..1bbc39d --- /dev/null +++ b/src/HAL/LPC1768/eeprom_wired.cpp @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +/** + * PersistentStore for Arduino-style EEPROM interface + * with implementations supplied by the framework. + */ + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x8000 // 32K +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + // Read from external EEPROM + const uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/endstop_interrupts.h b/src/HAL/LPC1768/endstop_interrupts.h new file mode 100644 index 0000000..23bd0cc --- /dev/null +++ b/src/HAL/LPC1768/endstop_interrupts.h @@ -0,0 +1,158 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + #define LPC1768_PIN_INTERRUPT_M(pin) ((pin >> 0x5 & 0x7) == 0 || (pin >> 0x5 & 0x7) == 2) + + #if HAS_X_MAX + #if !LPC1768_PIN_INTERRUPT_M(X_MAX_PIN) + #error "X_MAX_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(X_MAX_PIN); + #endif + #if HAS_X_MIN + #if !LPC1768_PIN_INTERRUPT_M(X_MIN_PIN) + #error "X_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(X_MIN_PIN); + #endif + #if HAS_Y_MAX + #if !LPC1768_PIN_INTERRUPT_M(Y_MAX_PIN) + #error "Y_MAX_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Y_MAX_PIN); + #endif + #if HAS_Y_MIN + #if !LPC1768_PIN_INTERRUPT_M(Y_MIN_PIN) + #error "Y_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Y_MIN_PIN); + #endif + #if HAS_Z_MAX + #if !LPC1768_PIN_INTERRUPT_M(Z_MAX_PIN) + #error "Z_MAX_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z_MAX_PIN); + #endif + #if HAS_Z_MIN + #if !LPC1768_PIN_INTERRUPT_M(Z_MIN_PIN) + #error "Z_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z_MIN_PIN); + #endif + #if HAS_Z2_MAX + #if !LPC1768_PIN_INTERRUPT_M(Z2_MAX_PIN) + #error "Z2_MAX_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z2_MAX_PIN); + #endif + #if HAS_Z2_MIN + #if !LPC1768_PIN_INTERRUPT_M(Z2_MIN_PIN) + #error "Z2_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z2_MIN_PIN); + #endif + #if HAS_Z3_MAX + #if !LPC1768_PIN_INTERRUPT_M(Z3_MAX_PIN) + #error "Z3_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z3_MAX_PIN); + #endif + #if HAS_Z3_MIN + #if !LPC1768_PIN_INTERRUPT_M(Z3_MIN_PIN) + #error "Z3_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z3_MIN_PIN); + #endif + #if HAS_Z4_MAX + #if !LPC1768_PIN_INTERRUPT_M(Z4_MAX_PIN) + #error "Z4_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z4_MAX_PIN); + #endif + #if HAS_Z4_MIN + #if !LPC1768_PIN_INTERRUPT_M(Z4_MIN_PIN) + #error "Z4_MIN_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z4_MIN_PIN); + #endif + #if HAS_Z_MIN_PROBE_PIN + #if !LPC1768_PIN_INTERRUPT_M(Z_MIN_PROBE_PIN) + #error "Z_MIN_PROBE_PIN is not INTERRUPT-capable. Disable ENDSTOP_INTERRUPTS_FEATURE to continue." + #endif + _ATTACH(Z_MIN_PROBE_PIN); + #endif + #if HAS_I_MAX + #if !LPC1768_PIN_INTERRUPT_M(I_MAX_PIN) + #error "I_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(I_MAX_PIN); + #elif HAS_I_MIN + #if !LPC1768_PIN_INTERRUPT_M(I_MIN_PIN) + #error "I_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(I_MIN_PIN); + #endif + #if HAS_J_MAX + #if !LPC1768_PIN_INTERRUPT_M(J_MAX_PIN) + #error "J_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(J_MAX_PIN); + #elif HAS_J_MIN + #if !LPC1768_PIN_INTERRUPT_M(J_MIN_PIN) + #error "J_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(J_MIN_PIN); + #endif + #if HAS_K_MAX + #if !LPC1768_PIN_INTERRUPT_M(K_MAX_PIN) + #error "K_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(K_MAX_PIN); + #elif HAS_K_MIN + #if !LPC1768_PIN_INTERRUPT_M(K_MIN_PIN) + #error "K_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(K_MIN_PIN); + #endif +} diff --git a/src/HAL/LPC1768/fast_pwm.cpp b/src/HAL/LPC1768/fast_pwm.cpp new file mode 100644 index 0000000..6d2b1a9 --- /dev/null +++ b/src/HAL/LPC1768/fast_pwm.cpp @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" +#include + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + if (!LPC176x::pin_is_valid(pin)) return; + if (LPC176x::pwm_attach_pin(pin)) + LPC176x::pwm_write_ratio(pin, invert ? 1.0f - (float)v / v_size : (float)v / v_size); // map 1-254 onto PWM range +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + LPC176x::pwm_set_frequency(pin, f_desired); +} + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/fastio.h b/src/HAL/LPC1768/fastio.h new file mode 100644 index 0000000..c553ffb --- /dev/null +++ b/src/HAL/LPC1768/fastio.h @@ -0,0 +1,119 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for LPC1768/9 + * Use direct port manipulation to save scads of processor time. + * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al. + */ + +/** + * Description: Fast IO functions LPC1768 + * + * For TARGET LPC1768 + */ + +#include "../shared/Marduino.h" + +#define PWM_PIN(P) true // all pins are PWM capable + +#define LPC_PIN(pin) LPC176x::gpio_pin(pin) +#define LPC_GPIO(port) LPC176x::gpio_port(port) + +#define SET_DIR_INPUT(IO) LPC176x::gpio_set_input(IO) +#define SET_DIR_OUTPUT(IO) LPC176x::gpio_set_output(IO) + +#define SET_MODE(IO, mode) pinMode(IO, mode) + +#define WRITE_PIN_SET(IO) LPC176x::gpio_set(IO) +#define WRITE_PIN_CLR(IO) LPC176x::gpio_clear(IO) + +#define READ_PIN(IO) LPC176x::gpio_get(IO) +#define WRITE_PIN(IO,V) LPC176x::gpio_set(IO, V) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(STEP); WRITE(STEP, HIGH); WRITE(STEP, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/gcc-4.8.5/cpp/Stringification.html + */ + +/// Read a pin +#define _READ(IO) READ_PIN(IO) + +/// Write to a pin +#define _WRITE(IO,V) WRITE_PIN(IO,V) + +/// toggle a pin +#define _TOGGLE(IO) _WRITE(IO, !READ(IO)) + +/// set pin as input +#define _SET_INPUT(IO) SET_DIR_INPUT(IO) + +/// set pin as output +#define _SET_OUTPUT(IO) SET_DIR_OUTPUT(IO) + +/// set pin as input with pullup mode +#define _PULLUP(IO,V) pinMode(IO, (V) ? INPUT_PULLUP : INPUT) + +/// set pin as input with pulldown mode +#define _PULLDOWN(IO,V) pinMode(IO, (V) ? INPUT_PULLDOWN : INPUT) + +/// check if pin is an input +#define _IS_INPUT(IO) (!LPC176x::gpio_get_dir(IO)) + +/// check if pin is an output +#define _IS_OUTPUT(IO) (LPC176x::gpio_get_dir(IO)) + +/// Read a pin wrapper +#define READ(IO) _READ(IO) + +/// Write to a pin wrapper +#define WRITE(IO,V) _WRITE(IO,V) + +/// toggle a pin wrapper +#define TOGGLE(IO) _TOGGLE(IO) + +/// set pin as input wrapper +#define SET_INPUT(IO) _SET_INPUT(IO) +/// set pin as input with pullup wrapper +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) +/// set pin as input with pulldown wrapper +#define SET_INPUT_PULLDOWN(IO) do{ _SET_INPUT(IO); _PULLDOWN(IO, HIGH); }while(0) +/// set pin as output wrapper - reads the pin and sets the output to that value +#define SET_OUTPUT(IO) do{ _WRITE(IO, _READ(IO)); _SET_OUTPUT(IO); }while(0) +// set pin as PWM +#define SET_PWM SET_OUTPUT + +/// check if pin is an input wrapper +#define IS_INPUT(IO) _IS_INPUT(IO) +/// check if pin is an output wrapper +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/src/HAL/LPC1768/inc/Conditionals_LCD.h b/src/HAL/LPC1768/inc/Conditionals_LCD.h new file mode 100644 index 0000000..32ef908 --- /dev/null +++ b/src/HAL/LPC1768/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_FSMC_TFT + #error "Sorry! FSMC TFT displays are not current available for HAL/LPC1768." +#endif diff --git a/src/HAL/LPC1768/inc/Conditionals_adv.h b/src/HAL/LPC1768/inc/Conditionals_adv.h new file mode 100644 index 0000000..8e7cab1 --- /dev/null +++ b/src/HAL/LPC1768/inc/Conditionals_adv.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if DISABLED(NO_SD_HOST_DRIVE) + #define HAS_SD_HOST_DRIVE 1 +#endif diff --git a/src/HAL/LPC1768/inc/Conditionals_post.h b/src/HAL/LPC1768/inc/Conditionals_post.h new file mode 100644 index 0000000..be574a9 --- /dev/null +++ b/src/HAL/LPC1768/inc/Conditionals_post.h @@ -0,0 +1,34 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define FLASH_EEPROM_EMULATION +#elif EITHER(I2C_EEPROM, SPI_EEPROM) + #define USE_SHARED_EEPROM 1 +#endif + +// LPC1768 boards seem to lose steps when saving to EEPROM during print (issue #20785) +// TODO: Which other boards are incompatible? +#if defined(MCU_LPC1768) && PRINTCOUNTER_SAVE_INTERVAL > 0 + #define PRINTCOUNTER_SYNC 1 +#endif diff --git a/src/HAL/LPC1768/inc/SanityCheck.h b/src/HAL/LPC1768/inc/SanityCheck.h new file mode 100644 index 0000000..8265d58 --- /dev/null +++ b/src/HAL/LPC1768/inc/SanityCheck.h @@ -0,0 +1,276 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if PIO_PLATFORM_VERSION < 1001 + #error "nxplpc-arduino-lpc176x package is out of date, Please update the PlatformIO platforms, frameworks and libraries. You may need to remove the platform and let it reinstall automatically." +#endif +#if PIO_FRAMEWORK_VERSION < 2006 + #error "framework-arduino-lpc176x package is out of date, Please update the PlatformIO platforms, frameworks and libraries." +#endif + +/** + * Detect an old pins file by checking for old ADC pins values. + */ +#define _OLD_TEMP_PIN(P) PIN_EXISTS(P) && _CAT(P,_PIN) <= 7 && !WITHIN(_CAT(P,_PIN), TERN(LPC1768_IS_SKRV1_3, 0, 2), 3) // Include P0_00 and P0_01 for SKR V1.3 board +#if _OLD_TEMP_PIN(TEMP_BED) + #error "TEMP_BED_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_0) + #error "TEMP_0_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_1) + #error "TEMP_1_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_2) + #error "TEMP_2_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_3) + #error "TEMP_3_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_4) + #error "TEMP_4_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_5) + #error "TEMP_5_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_6) + #error "TEMP_6_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#elif _OLD_TEMP_PIN(TEMP_7) + #error "TEMP_7_PIN must be defined using the Pn_nn or Pn_nn_An format. (See the included pins files)." +#endif +#undef _OLD_TEMP_PIN + +/** + * Because PWM hardware channels all share the same frequency, along with the + * fallback software channels, FAST_PWM_FAN is incompatible with Servos. + */ +static_assert(!(NUM_SERVOS && ENABLED(FAST_PWM_FAN)), "BLTOUCH and Servos are incompatible with FAST_PWM_FAN on LPC176x boards."); + +#if SPINDLE_LASER_FREQUENCY + static_assert(!NUM_SERVOS, "BLTOUCH and Servos are incompatible with SPINDLE_LASER_FREQUENCY on LPC176x boards."); +#endif + +/** + * Test LPC176x-specific configuration values for errors at compile-time. + */ + +//#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) +// #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" +//#endif + +#if MB(RAMPS_14_RE_ARM_EFB, RAMPS_14_RE_ARM_EEB, RAMPS_14_RE_ARM_EFF, RAMPS_14_RE_ARM_EEF, RAMPS_14_RE_ARM_SF) + #if IS_RRD_FG_SC && HAS_DRIVER(TMC2130) && DISABLED(TMC_USE_SW_SPI) + #error "Re-ARM with REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER and TMC2130 requires TMC_USE_SW_SPI." + #endif +#endif + +static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported on LPC176x."); + +/** + * Flag any serial port conflicts + * + * Port | TX | RX | + * --- | --- | --- | + * Serial | P0_02 | P0_03 | + * Serial1 | P0_15 | P0_16 | + * Serial2 | P0_10 | P0_11 | + * Serial3 | P0_00 | P0_01 | + */ +#define ANY_TX(N,V...) DO(IS_TX##N,||,V) +#define ANY_RX(N,V...) DO(IS_RX##N,||,V) + +#if USING_HW_SERIAL0 + #define IS_TX0(P) (P == P0_02) + #define IS_RX0(P) (P == P0_03) + #if IS_TX0(TMC_SW_MISO) || IS_RX0(TMC_SW_MOSI) + #error "Serial port pins (0) conflict with Trinamic SPI pins!" + #elif HAS_PRUSA_MMU1 && (IS_TX0(E_MUX1_PIN) || IS_RX0(E_MUX0_PIN)) + #error "Serial port pins (0) conflict with Multi-Material-Unit multiplexer pins!" + #elif (AXIS_HAS_SPI(X) && IS_TX0(X_CS_PIN)) || (AXIS_HAS_SPI(Y) && IS_RX0(Y_CS_PIN)) + #error "Serial port pins (0) conflict with X/Y axis SPI pins!" + #endif + #undef IS_TX0 + #undef IS_RX0 +#endif + +#if USING_HW_SERIAL1 + #define IS_TX1(P) (P == P0_15) + #define IS_RX1(P) (P == P0_16) + #define _IS_TX1_1 IS_TX1 + #define _IS_RX1_1 IS_RX1 + #if IS_TX1(TMC_SW_SCK) + #error "Serial port pins (1) conflict with other pins!" + #elif HAS_ROTARY_ENCODER + #if IS_TX1(BTN_EN2) || IS_RX1(BTN_EN1) + #error "Serial port pins (1) conflict with Encoder Buttons!" + #elif ANY_TX(1, SD_SCK_PIN, LCD_PINS_D4, DOGLCD_SCK, LCD_RESET_PIN, LCD_PINS_RS, SHIFT_CLK_PIN) \ + || ANY_RX(1, LCD_SDSS, LCD_PINS_RS, SD_MISO_PIN, DOGLCD_A0, SD_SS_PIN, LCD_SDSS, DOGLCD_CS, LCD_RESET_PIN, LCD_BACKLIGHT_PIN) + #error "Serial port pins (1) conflict with LCD pins!" + #endif + #endif + #undef IS_TX1 + #undef IS_RX1 + #undef _IS_TX1_1 + #undef _IS_RX1_1 +#endif + +#if USING_HW_SERIAL2 + #define IS_TX2(P) (P == P0_10) + #define IS_RX2(P) (P == P0_11) + #define _IS_TX2_1 IS_TX2 + #define _IS_RX2_1 IS_RX2 + #if IS_TX2(X2_ENABLE_PIN) || ANY_RX(2, X2_DIR_PIN, X2_STEP_PIN) || (AXIS_HAS_SPI(X2) && IS_TX2(X2_CS_PIN)) + #error "Serial port pins (2) conflict with X2 pins!" + #elif IS_TX2(Y2_ENABLE_PIN) || ANY_RX(2, Y2_DIR_PIN, Y2_STEP_PIN) || (AXIS_HAS_SPI(Y2) && IS_TX2(Y2_CS_PIN)) + #error "Serial port pins (2) conflict with Y2 pins!" + #elif IS_TX2(Z2_ENABLE_PIN) || ANY_RX(2, Z2_DIR_PIN, Z2_STEP_PIN) || (AXIS_HAS_SPI(Z2) && IS_TX2(Z2_CS_PIN)) + #error "Serial port pins (2) conflict with Z2 pins!" + #elif IS_TX2(Z3_ENABLE_PIN) || ANY_RX(2, Z3_DIR_PIN, Z3_STEP_PIN) || (AXIS_HAS_SPI(Z3) && IS_TX2(Z3_CS_PIN)) + #error "Serial port pins (2) conflict with Z3 pins!" + #elif IS_TX2(Z4_ENABLE_PIN) || ANY_RX(2, Z4_DIR_PIN, Z4_STEP_PIN) || (AXIS_HAS_SPI(Z4) && IS_TX2(Z4_CS_PIN)) + #error "Serial port pins (2) conflict with Z4 pins!" + #elif ANY_RX(2, X_DIR_PIN, Y_DIR_PIN) + #error "Serial port pins (2) conflict with other pins!" + #elif Y_HOME_TO_MIN && IS_TX2(Y_STOP_PIN) + #error "Serial port pins (2) conflict with Y endstop pin!" + #elif USES_Z_MIN_PROBE_PIN && IS_TX2(Z_MIN_PROBE_PIN) + #error "Serial port pins (2) conflict with probe pin!" + #elif ANY_TX(2, X_ENABLE_PIN, Y_ENABLE_PIN) || ANY_RX(2, X_DIR_PIN, Y_DIR_PIN) + #error "Serial port pins (2) conflict with X/Y stepper pins!" + #elif HAS_MULTI_EXTRUDER && (IS_TX2(E1_ENABLE_PIN) || (AXIS_HAS_SPI(E1) && IS_TX2(E1_CS_PIN))) + #error "Serial port pins (2) conflict with E1 stepper pins!" + #elif EXTRUDERS && ANY_RX(2, E0_DIR_PIN, E0_STEP_PIN) + #error "Serial port pins (2) conflict with E stepper pins!" + #endif + #undef IS_TX2 + #undef IS_RX2 + #undef _IS_TX2_1 + #undef _IS_RX2_1 +#endif + +#if USING_HW_SERIAL3 + #define PIN_IS_TX3(P) (PIN_EXISTS(P) && P##_PIN == P0_00) + #define PIN_IS_RX3(P) (P##_PIN == P0_01) + #if PIN_IS_TX3(X_MIN) || PIN_IS_RX3(X_MAX) + #error "Serial port pins (3) conflict with X endstop pins!" + #elif PIN_IS_TX3(Y_SERIAL_TX) || PIN_IS_TX3(Y_SERIAL_RX) || PIN_IS_RX3(X_SERIAL_TX) || PIN_IS_RX3(X_SERIAL_RX) + #error "Serial port pins (3) conflict with X/Y axis UART pins!" + #elif PIN_IS_TX3(X2_DIR) || PIN_IS_RX3(X2_STEP) + #error "Serial port pins (3) conflict with X2 pins!" + #elif PIN_IS_TX3(Y2_DIR) || PIN_IS_RX3(Y2_STEP) + #error "Serial port pins (3) conflict with Y2 pins!" + #elif PIN_IS_TX3(Z2_DIR) || PIN_IS_RX3(Z2_STEP) + #error "Serial port pins (3) conflict with Z2 pins!" + #elif PIN_IS_TX3(Z3_DIR) || PIN_IS_RX3(Z3_STEP) + #error "Serial port pins (3) conflict with Z3 pins!" + #elif PIN_IS_TX3(Z4_DIR) || PIN_IS_RX3(Z4_STEP) + #error "Serial port pins (3) conflict with Z4 pins!" + #elif HAS_MULTI_EXTRUDER && (PIN_IS_TX3(E1_DIR) || PIN_IS_RX3(E1_STEP)) + #error "Serial port pins (3) conflict with E1 pins!" + #endif + #undef PIN_IS_TX3 + #undef PIN_IS_RX3 +#endif + +#undef ANY_TX +#undef ANY_RX + +// +// Flag any i2c pin conflicts +// +#if ANY(HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC, EXPERIMENTAL_I2CBUS, I2C_POSITION_ENCODERS, PCA9632, I2C_EEPROM) + #define USEDI2CDEV_M 1 // /Wire.cpp + + #if USEDI2CDEV_M == 0 // P0_27 [D57] (AUX-1) .......... P0_28 [D58] (AUX-1) + #define PIN_IS_SDA0(P) (P##_PIN == P0_27) + #define IS_SCL0(P) (P == P0_28) + #if ENABLED(SDSUPPORT) && PIN_IS_SDA0(SD_DETECT) + #error "SDA0 overlaps with SD_DETECT_PIN!" + #elif PIN_IS_SDA0(E0_AUTO_FAN) + #error "SDA0 overlaps with E0_AUTO_FAN_PIN!" + #elif PIN_IS_SDA0(BEEPER) + #error "SDA0 overlaps with BEEPER_PIN!" + #elif IS_SCL0(BTN_ENC) + #error "SCL0 overlaps with Encoder Button!" + #elif IS_SCL0(SD_SS_PIN) + #error "SCL0 overlaps with SD_SS_PIN!" + #elif IS_SCL0(LCD_SDSS) + #error "SCL0 overlaps with LCD_SDSS!" + #endif + #undef PIN_IS_SDA0 + #undef IS_SCL0 + #elif USEDI2CDEV_M == 1 // P0_00 [D20] (SCA) ............ P0_01 [D21] (SCL) + #define PIN_IS_SDA1(P) (PIN_EXISTS(P) && P##_PIN == P0_00) + #define PIN_IS_SCL1(P) (P##_PIN == P0_01) + #if PIN_IS_SDA1(X_MIN) || PIN_IS_SCL1(X_MAX) + #error "One or more i2c (1) pins overlaps with X endstop pins! Disable i2c peripherals." + #elif PIN_IS_SDA1(X2_DIR) || PIN_IS_SCL1(X2_STEP) + #error "One or more i2c (1) pins overlaps with X2 pins! Disable i2c peripherals." + #elif PIN_IS_SDA1(Y2_DIR) || PIN_IS_SCL1(Y2_STEP) + #error "One or more i2c (1) pins overlaps with Y2 pins! Disable i2c peripherals." + #elif PIN_IS_SDA1(Z2_DIR) || PIN_IS_SCL1(Z2_STEP) + #error "One or more i2c (1) pins overlaps with Z2 pins! Disable i2c peripherals." + #elif PIN_IS_SDA1(Z3_DIR) || PIN_IS_SCL1(Z3_STEP) + #error "One or more i2c (1) pins overlaps with Z3 pins! Disable i2c peripherals." + #elif PIN_IS_SDA1(Z4_DIR) || PIN_IS_SCL1(Z4_STEP) + #error "One or more i2c (1) pins overlaps with Z4 pins! Disable i2c peripherals." + #elif HAS_MULTI_EXTRUDER && (PIN_IS_SDA1(E1_DIR) || PIN_IS_SCL1(E1_STEP)) + #error "One or more i2c (1) pins overlaps with E1 pins! Disable i2c peripherals." + #endif + #undef PIN_IS_SDA1 + #undef PIN_IS_SCL1 + #elif USEDI2CDEV_M == 2 // P0_10 [D38] (X_ENABLE_PIN) ... P0_11 [D55] (X_DIR_PIN) + #define PIN_IS_SDA2(P) (P##_PIN == P0_10) + #define PIN_IS_SCL2(P) (P##_PIN == P0_11) + #if PIN_IS_SDA2(Y_STOP) + #error "i2c SDA2 overlaps with Y endstop pin!" + #elif USES_Z_MIN_PROBE_PIN && PIN_IS_SDA2(Z_MIN_PROBE) + #error "i2c SDA2 overlaps with Z probe pin!" + #elif PIN_IS_SDA2(X_ENABLE) || PIN_IS_SDA2(Y_ENABLE) + #error "i2c SDA2 overlaps with X/Y ENABLE pin!" + #elif AXIS_HAS_SPI(X) && PIN_IS_SDA2(X_CS) + #error "i2c SDA2 overlaps with X CS pin!" + #elif PIN_IS_SDA2(X2_ENABLE) + #error "i2c SDA2 overlaps with X2 enable pin! Disable i2c peripherals." + #elif PIN_IS_SDA2(Y2_ENABLE) + #error "i2c SDA2 overlaps with Y2 enable pin! Disable i2c peripherals." + #elif PIN_IS_SDA2(Z2_ENABLE) + #error "i2c SDA2 overlaps with Z2 enable pin! Disable i2c peripherals." + #elif PIN_IS_SDA2(Z3_ENABLE) + #error "i2c SDA2 overlaps with Z3 enable pin! Disable i2c peripherals." + #elif PIN_IS_SDA2(Z4_ENABLE) + #error "i2c SDA2 overlaps with Z4 enable pin! Disable i2c peripherals." + #elif HAS_MULTI_EXTRUDER && PIN_IS_SDA2(E1_ENABLE) + #error "i2c SDA2 overlaps with E1 enable pin! Disable i2c peripherals." + #elif HAS_MULTI_EXTRUDER && AXIS_HAS_SPI(E1) && PIN_IS_SDA2(E1_CS) + #error "i2c SDA2 overlaps with E1 CS pin! Disable i2c peripherals." + #elif EXTRUDERS && (PIN_IS_SDA2(E0_STEP) || PIN_IS_SDA2(E0_DIR)) + #error "i2c SCL2 overlaps with E0 STEP/DIR pin! Disable i2c peripherals." + #elif PIN_IS_SDA2(X_DIR) || PIN_IS_SDA2(Y_DIR) + #error "One or more i2c pins overlaps with X/Y DIR pin! Disable i2c peripherals." + #endif + #undef PIN_IS_SDA2 + #undef PIN_IS_SCL2 + #endif + + #undef USEDI2CDEV_M +#endif + +#if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + #error "SERIAL_STATS_MAX_RX_QUEUED is not supported on LPC176x." +#elif ENABLED(SERIAL_STATS_DROPPED_RX) + #error "SERIAL_STATS_DROPPED_RX is not supported on LPX176x." +#endif diff --git a/src/HAL/LPC1768/include/SPI.h b/src/HAL/LPC1768/include/SPI.h new file mode 100644 index 0000000..24f4759 --- /dev/null +++ b/src/HAL/LPC1768/include/SPI.h @@ -0,0 +1,182 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../shared/HAL_SPI.h" + +#include +#include +#include + +//#define MSBFIRST 1 + +#define SPI_MODE0 0 +#define SPI_MODE1 1 +#define SPI_MODE2 2 +#define SPI_MODE3 3 + +#define DATA_SIZE_8BIT SSP_DATABIT_8 +#define DATA_SIZE_16BIT SSP_DATABIT_16 + +#define SPI_CLOCK_MAX_TFT 30000000UL +#define SPI_CLOCK_DIV2 8333333 //(SCR: 2) desired: 8,000,000 actual: 8,333,333 +4.2% SPI_FULL_SPEED +#define SPI_CLOCK_DIV4 4166667 //(SCR: 5) desired: 4,000,000 actual: 4,166,667 +4.2% SPI_HALF_SPEED +#define SPI_CLOCK_DIV8 2083333 //(SCR: 11) desired: 2,000,000 actual: 2,083,333 +4.2% SPI_QUARTER_SPEED +#define SPI_CLOCK_DIV16 1000000 //(SCR: 24) desired: 1,000,000 actual: 1,000,000 SPI_EIGHTH_SPEED +#define SPI_CLOCK_DIV32 500000 //(SCR: 49) desired: 500,000 actual: 500,000 SPI_SPEED_5 +#define SPI_CLOCK_DIV64 250000 //(SCR: 99) desired: 250,000 actual: 250,000 SPI_SPEED_6 +#define SPI_CLOCK_DIV128 125000 //(SCR:199) desired: 125,000 actual: 125,000 Default from HAL.h + +#define SPI_CLOCK_MAX SPI_CLOCK_DIV2 + +#define BOARD_NR_SPI 2 + +//#define BOARD_SPI1_NSS_PIN PA4 ?! +#define BOARD_SPI1_SCK_PIN P0_15 +#define BOARD_SPI1_MISO_PIN P0_17 +#define BOARD_SPI1_MOSI_PIN P0_18 + +//#define BOARD_SPI2_NSS_PIN PB12 ?! +#define BOARD_SPI2_SCK_PIN P0_07 +#define BOARD_SPI2_MISO_PIN P0_08 +#define BOARD_SPI2_MOSI_PIN P0_09 + +class SPISettings { +public: + SPISettings(uint32_t spiRate, int inBitOrder, int inDataMode) { + init_AlwaysInline(spiRate2Clock(spiRate), inBitOrder, inDataMode, DATA_SIZE_8BIT); + } + SPISettings(uint32_t inClock, uint8_t inBitOrder, uint8_t inDataMode, uint32_t inDataSize) { + if (__builtin_constant_p(inClock)) + init_AlwaysInline(inClock, inBitOrder, inDataMode, inDataSize); + else + init_MightInline(inClock, inBitOrder, inDataMode, inDataSize); + } + SPISettings() { + init_AlwaysInline(4000000, MSBFIRST, SPI_MODE0, DATA_SIZE_8BIT); + } + + //uint32_t spiRate() const { return spi_speed; } + + static uint32_t spiRate2Clock(uint32_t spiRate) { + uint32_t Marlin_speed[7]; // CPSR is always 2 + Marlin_speed[0] = 8333333; //(SCR: 2) desired: 8,000,000 actual: 8,333,333 +4.2% SPI_FULL_SPEED + Marlin_speed[1] = 4166667; //(SCR: 5) desired: 4,000,000 actual: 4,166,667 +4.2% SPI_HALF_SPEED + Marlin_speed[2] = 2083333; //(SCR: 11) desired: 2,000,000 actual: 2,083,333 +4.2% SPI_QUARTER_SPEED + Marlin_speed[3] = 1000000; //(SCR: 24) desired: 1,000,000 actual: 1,000,000 SPI_EIGHTH_SPEED + Marlin_speed[4] = 500000; //(SCR: 49) desired: 500,000 actual: 500,000 SPI_SPEED_5 + Marlin_speed[5] = 250000; //(SCR: 99) desired: 250,000 actual: 250,000 SPI_SPEED_6 + Marlin_speed[6] = 125000; //(SCR:199) desired: 125,000 actual: 125,000 Default from HAL.h + return Marlin_speed[spiRate > 6 ? 6 : spiRate]; + } + +private: + void init_MightInline(uint32_t inClock, uint8_t inBitOrder, uint8_t inDataMode, uint32_t inDataSize) { + init_AlwaysInline(inClock, inBitOrder, inDataMode, inDataSize); + } + void init_AlwaysInline(uint32_t inClock, uint8_t inBitOrder, uint8_t inDataMode, uint32_t inDataSize) __attribute__((__always_inline__)) { + clock = inClock; + bitOrder = inBitOrder; + dataMode = inDataMode; + dataSize = inDataSize; + } + + //uint32_t spi_speed; + uint32_t clock; + uint32_t dataSize; + //uint32_t clockDivider; + uint8_t bitOrder; + uint8_t dataMode; + LPC_SSP_TypeDef *spi_d; + + friend class SPIClass; +}; + +/** + * @brief Wirish SPI interface. + * + * This is the same interface is available across HAL + * + * This implementation uses software slave management, so the caller + * is responsible for controlling the slave select line. + */ +class SPIClass { +public: + /** + * @param spiPortNumber Number of the SPI port to manage. + */ + SPIClass(uint8_t spiPortNumber); + + /** + * Init using pins + */ + SPIClass(pin_t mosi, pin_t miso, pin_t sclk, pin_t ssel = (pin_t)-1); + + /** + * Select and configure the current selected SPI device to use + */ + void begin(); + + /** + * Disable the current SPI device + */ + void end(); + + void beginTransaction(const SPISettings&); + void endTransaction() {} + + // Transfer using 1 "Data Size" + uint8_t transfer(uint16_t data); + // Transfer 2 bytes in 8 bit mode + uint16_t transfer16(uint16_t data); + + void send(uint8_t data); + + uint16_t read(); + void read(uint8_t *buf, uint32_t len); + + void dmaSend(void *buf, uint16_t length, bool minc); + + /** + * @brief Sets the number of the SPI peripheral to be used by + * this HardwareSPI instance. + * + * @param spi_num Number of the SPI port. 1-2 in low density devices + * or 1-3 in high density devices. + */ + void setModule(uint8_t device); + + void setClock(uint32_t clock); + void setBitOrder(uint8_t bitOrder); + void setDataMode(uint8_t dataMode); + void setDataSize(uint32_t ds); + + inline uint32_t getDataSize() { return _currentSetting->dataSize; } + +private: + SPISettings _settings[BOARD_NR_SPI]; + SPISettings *_currentSetting; + + void updateSettings(); +}; + +extern SPIClass SPI; diff --git a/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.c b/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.c new file mode 100644 index 0000000..c489c16 --- /dev/null +++ b/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.c @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * digipot_mcp4451_I2C_routines.c + * Adapted from https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if ENABLED(DIGIPOT_MCP4451) && MB(MKS_SBASE) + +#ifdef __cplusplus + extern "C" { +#endif + +#include "digipot_mcp4451_I2C_routines.h" + +uint8_t digipot_mcp4451_start(uint8_t sla) { // send slave address and write bit + // Sometimes TX data ACK or NAK status is returned. That mean the start state didn't + // happen which means only the value of the slave address was send. Keep looping until + // the slave address and write bit are actually sent. + do { + _I2C_Stop(I2CDEV_M); // output stop state on I2C bus + _I2C_Start(I2CDEV_M); // output start state on I2C bus + while ((I2C_status != I2C_I2STAT_M_TX_START) + && (I2C_status != I2C_I2STAT_M_TX_RESTART) + && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK)); //wait for start to be asserted + + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_STAC; // clear start state before tansmitting slave address + LPC_I2C1->I2DAT = (sla << 1) & I2C_I2DAT_BITMASK; // transmit slave address & write bit + LPC_I2C1->I2CONSET = I2C_I2CONSET_AA; + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC; + while ((I2C_status != I2C_I2STAT_M_TX_SLAW_ACK) + && (I2C_status != I2C_I2STAT_M_TX_SLAW_NACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK)) { /* wait for slaw to finish */ } + } while ( (I2C_status == I2C_I2STAT_M_TX_DAT_ACK) || (I2C_status == I2C_I2STAT_M_TX_DAT_NACK)); + return 1; +} + +uint8_t digipot_mcp4451_send_byte(uint8_t data) { + LPC_I2C1->I2DAT = data & I2C_I2DAT_BITMASK; // transmit data + LPC_I2C1->I2CONSET = I2C_I2CONSET_AA; + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC; + while (I2C_status != I2C_I2STAT_M_TX_DAT_ACK && I2C_status != I2C_I2STAT_M_TX_DAT_NACK); // wait for xmit to finish + return 1; +} + +#ifdef __cplusplus + } +#endif + +#endif // DIGIPOT_MCP4451 && MKS_SBASE +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.h b/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.h new file mode 100644 index 0000000..9b6c62b --- /dev/null +++ b/src/HAL/LPC1768/include/digipot_mcp4451_I2C_routines.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * digipot_mcp4451_I2C_routines.h + * Adapted from https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html + */ + +#ifdef __cplusplus + extern "C" { +#endif + +#include +#include +#include +#include "i2c_util.h" + +uint8_t digipot_mcp4451_start(uint8_t sla); +uint8_t digipot_mcp4451_send_byte(uint8_t data); + +#ifdef __cplusplus + } +#endif diff --git a/src/HAL/LPC1768/include/i2c_util.c b/src/HAL/LPC1768/include/i2c_util.c new file mode 100644 index 0000000..4e24f23 --- /dev/null +++ b/src/HAL/LPC1768/include/i2c_util.c @@ -0,0 +1,96 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL_LPC1768/include/i2c_util.c + */ + +#ifdef TARGET_LPC1768 + +#include "i2c_util.h" + +#define U8G_I2C_OPT_FAST 16 // from u8g.h + +#ifdef __cplusplus + extern "C" { +#endif + +void configure_i2c(const uint8_t clock_option) { + /** + * Init I2C pin connect + */ + PINSEL_CFG_Type PinCfg; + PinCfg.OpenDrain = 0; + PinCfg.Pinmode = 0; + PinCfg.Portnum = 0; + #if I2C_MASTER_ID == 0 + PinCfg.Funcnum = 1; + PinCfg.Pinnum = 27; // SDA0 / D57 AUX-1 ... SCL0 / D58 AUX-1 + #elif I2C_MASTER_ID == 1 + PinCfg.Funcnum = 3; + PinCfg.Pinnum = 0; // SDA1 / D20 SCA ... SCL1 / D21 SCL + #elif I2C_MASTER_ID == 2 + PinCfg.Funcnum = 2; + PinCfg.Pinnum = 10; // SDA2 / D38 X_ENABLE_PIN ... SCL2 / D55 X_DIR_PIN + #endif + PINSEL_ConfigPin(&PinCfg); + PinCfg.Pinnum += 1; + PINSEL_ConfigPin(&PinCfg); + + // Initialize I2C peripheral + I2C_Init(I2CDEV_M, (clock_option & U8G_I2C_OPT_FAST) ? 400000: 100000); // LCD data rates + + // Enable Master I2C operation + I2C_Cmd(I2CDEV_M, I2C_MASTER_MODE, ENABLE); +} + +////////////////////////////////////////////////////////////////////////////////////// +// These two routines are exact copies of the lpc17xx_i2c.c routines. Couldn't link to +// to the lpc17xx_i2c.c routines so had to copy them into this file & rename them. + +uint32_t _I2C_Start(LPC_I2C_TypeDef *I2Cx) { + // Reset STA, STO, SI + I2Cx->I2CONCLR = I2C_I2CONCLR_SIC|I2C_I2CONCLR_STOC|I2C_I2CONCLR_STAC; + + // Enter to Master Transmitter mode + I2Cx->I2CONSET = I2C_I2CONSET_STA; + + // Wait for complete + while (!(I2Cx->I2CONSET & I2C_I2CONSET_SI)); + I2Cx->I2CONCLR = I2C_I2CONCLR_STAC; + return (I2Cx->I2STAT & I2C_STAT_CODE_BITMASK); +} + +void _I2C_Stop(LPC_I2C_TypeDef *I2Cx) { + /* Make sure start bit is not active */ + if (I2Cx->I2CONSET & I2C_I2CONSET_STA) + I2Cx->I2CONCLR = I2C_I2CONCLR_STAC; + + I2Cx->I2CONSET = I2C_I2CONSET_STO|I2C_I2CONSET_AA; + I2Cx->I2CONCLR = I2C_I2CONCLR_SIC; +} + +#ifdef __cplusplus + } +#endif + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/include/i2c_util.h b/src/HAL/LPC1768/include/i2c_util.h new file mode 100644 index 0000000..1f1c19f --- /dev/null +++ b/src/HAL/LPC1768/include/i2c_util.h @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL_LPC1768/include/i2c_util.h + */ + +#include "../../../inc/MarlinConfigPre.h" + +#ifndef I2C_MASTER_ID + #define I2C_MASTER_ID 1 +#endif + +#if I2C_MASTER_ID == 0 + #define I2CDEV_M LPC_I2C0 +#elif I2C_MASTER_ID == 1 + #define I2CDEV_M LPC_I2C1 +#elif I2C_MASTER_ID == 2 + #define I2CDEV_M LPC_I2C2 +#else + #error "Master I2C device not defined!" +#endif + +#include +#include +#include + +#ifdef __cplusplus + extern "C" { +#endif + +void configure_i2c(const uint8_t clock_option); + +uint32_t _I2C_Start(LPC_I2C_TypeDef *I2Cx); +void _I2C_Stop(LPC_I2C_TypeDef *I2Cx); + +#define I2C_status (LPC_I2C1->I2STAT & I2C_STAT_CODE_BITMASK) + +#ifdef __cplusplus + } +#endif diff --git a/src/HAL/LPC1768/main.cpp b/src/HAL/LPC1768/main.cpp new file mode 100644 index 0000000..419c997 --- /dev/null +++ b/src/HAL/LPC1768/main.cpp @@ -0,0 +1,163 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../../inc/MarlinConfig.h" +#include "../../core/millis_t.h" + +#include "../../sd/cardreader.h" + +extern uint32_t MSC_SD_Init(uint8_t pdrv); + +extern "C" { + #include + extern "C" int isLPC1769(); + extern "C" void disk_timerproc(); +} + +void SysTick_Callback() { disk_timerproc(); } + +TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); + +void MarlinHAL::init() { + + // Init LEDs + #if PIN_EXISTS(LED) + SET_DIR_OUTPUT(LED_PIN); + WRITE_PIN_CLR(LED_PIN); + #if PIN_EXISTS(LED2) + SET_DIR_OUTPUT(LED2_PIN); + WRITE_PIN_CLR(LED2_PIN); + #if PIN_EXISTS(LED3) + SET_DIR_OUTPUT(LED3_PIN); + WRITE_PIN_CLR(LED3_PIN); + #if PIN_EXISTS(LED4) + SET_DIR_OUTPUT(LED4_PIN); + WRITE_PIN_CLR(LED4_PIN); + #endif + #endif + #endif + + // Flash status LED 3 times to indicate Marlin has started booting + LOOP_L_N(i, 6) { + TOGGLE(LED_PIN); + delay(100); + } + #endif + + // Init Servo Pins + #define INIT_SERVO(N) OUT_WRITE(SERVO##N##_PIN, LOW) + #if HAS_SERVO_0 + INIT_SERVO(0); + #endif + #if HAS_SERVO_1 + INIT_SERVO(1); + #endif + #if HAS_SERVO_2 + INIT_SERVO(2); + #endif + #if HAS_SERVO_3 + INIT_SERVO(3); + #endif + + //debug_frmwrk_init(); + //_DBG("\n\nDebug running\n"); + // Initialize the SD card chip select pins as soon as possible + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + + #if PIN_EXISTS(ONBOARD_SD_CS) && ONBOARD_SD_CS_PIN != SD_SS_PIN + OUT_WRITE(ONBOARD_SD_CS_PIN, HIGH); + #endif + + #ifdef LPC1768_ENABLE_CLKOUT_12M + /** + * CLKOUTCFG register + * bit 8 (CLKOUT_EN) = enables CLKOUT signal. Disabled for now to prevent glitch when enabling GPIO. + * bits 7:4 (CLKOUTDIV) = set to 0 for divider setting of /1 + * bits 3:0 (CLKOUTSEL) = set to 1 to select main crystal oscillator as CLKOUT source + */ + LPC_SC->CLKOUTCFG = (0<<8)|(0<<4)|(1<<0); + // set P1.27 pin to function 01 (CLKOUT) + PINSEL_CFG_Type PinCfg; + PinCfg.Portnum = 1; + PinCfg.Pinnum = 27; + PinCfg.Funcnum = 1; // function 01 (CLKOUT) + PinCfg.OpenDrain = 0; // not open drain + PinCfg.Pinmode = 2; // no pull-up/pull-down + PINSEL_ConfigPin(&PinCfg); + // now set CLKOUT_EN bit + SBI(LPC_SC->CLKOUTCFG, 8); + #endif + + USB_Init(); // USB Initialization + USB_Connect(false); // USB clear connection + delay(1000); // Give OS time to notice + USB_Connect(true); + + TERN_(HAS_SD_HOST_DRIVE, MSC_SD_Init(0)); // Enable USB SD card access + + const millis_t usb_timeout = millis() + 2000; + while (!USB_Configuration && PENDING(millis(), usb_timeout)) { + delay(50); + idletask(); + #if PIN_EXISTS(LED) + TOGGLE(LED_PIN); // Flash quickly during USB initialization + #endif + } + + HAL_timer_init(); + + TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler +} + +// HAL idle task +void MarlinHAL::idletask() { + #if HAS_SHARED_MEDIA + // If Marlin is using the SD card we need to lock it to prevent access from + // a PC via USB. + // Other HALs use IS_SD_PRINTING() and IS_SD_FILE_OPEN() to check for access but + // this will not reliably detect delete operations. To be safe we will lock + // the disk if Marlin has it mounted. Unfortunately there is currently no way + // to unmount the disk from the LCD menu. + // if (IS_SD_PRINTING() || IS_SD_FILE_OPEN()) + if (card.isMounted()) + MSC_Aquire_Lock(); + else + MSC_Release_Lock(); + #endif + // Perform USB stack housekeeping + MSC_RunDeferredCommands(); +} + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/pinsDebug.h b/src/HAL/LPC1768/pinsDebug.h new file mode 100644 index 0000000..a2f5c12 --- /dev/null +++ b/src/HAL/LPC1768/pinsDebug.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Support routines for LPC1768 + */ + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS +#define pwm_details(pin) pin = pin // do nothing // print PWM details +#define pwm_status(pin) false //Print a pin's PWM status. Return true if it's currently a PWM pin. +#define IS_ANALOG(P) (DIGITAL_PIN_TO_ANALOG_PIN(P) >= 0 ? 1 : 0) +#define digitalRead_mod(p) extDigitalRead(p) +#define PRINT_PORT(p) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("P%d_%02d"), LPC176x::pin_port(p), LPC176x::pin_bit(p)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR("_A%d "), LPC176x::pin_get_adc_channel(pin)); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 17 // space needed to be pretty if not first name assigned to a pin + +// pins that will cause hang/reset/disconnect in M43 Toggle and Watch utilities +#ifndef M43_NEVER_TOUCH + #define M43_NEVER_TOUCH(Q) ((Q) == P0_29 || (Q) == P0_30 || (Q) == P2_09) // USB pins +#endif + +bool GET_PINMODE(const pin_t pin) { + if (!LPC176x::pin_is_valid(pin) || LPC176x::pin_adc_enabled(pin)) // found an invalid pin or active analog pin + return false; + + return LPC176x::gpio_direction(pin); +} + +#define GET_ARRAY_IS_DIGITAL(x) ((bool) pin_array[x].is_digital) diff --git a/src/HAL/LPC1768/spi_pins.h b/src/HAL/LPC1768/spi_pins.h new file mode 100644 index 0000000..e7d7747 --- /dev/null +++ b/src/HAL/LPC1768/spi_pins.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/macros.h" + +#if BOTH(SDSUPPORT, HAS_MARLINUI_U8GLIB) && (LCD_PINS_D4 == SD_SCK_PIN || LCD_PINS_ENABLE == SD_MOSI_PIN || DOGLCD_SCK == SD_SCK_PIN || DOGLCD_MOSI == SD_MOSI_PIN) + #define LPC_SOFTWARE_SPI // If the SD card and LCD adapter share the same SPI pins, then software SPI is currently + // needed due to the speed and mode required for communicating with each device being different. + // This requirement can be removed if the SPI access to these devices is updated to use + // spiBeginTransaction. +#endif + +/** onboard SD card */ +//#define SD_SCK_PIN P0_07 +//#define SD_MISO_PIN P0_08 +//#define SD_MOSI_PIN P0_09 +//#define SD_SS_PIN P0_06 +/** external */ +#ifndef SD_SCK_PIN + #define SD_SCK_PIN P0_15 +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN P0_17 +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN P0_18 +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN P1_23 +#endif +#if !defined(SDSS) || SDSS == P_NC // gets defaulted in pins.h + #undef SDSS + #define SDSS SD_SS_PIN +#endif diff --git a/src/HAL/LPC1768/tft/tft_spi.cpp b/src/HAL/LPC1768/tft/tft_spi.cpp new file mode 100644 index 0000000..a9847b2 --- /dev/null +++ b/src/HAL/LPC1768/tft/tft_spi.cpp @@ -0,0 +1,130 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_SPI_TFT + +#include "tft_spi.h" + +SPIClass TFT_SPI::SPIx(1); + +void TFT_SPI::Init() { + #if PIN_EXISTS(TFT_RESET) + OUT_WRITE(TFT_RESET_PIN, HIGH); + delay(100); + #endif + + #if PIN_EXISTS(TFT_BACKLIGHT) + OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); + #endif + + SET_OUTPUT(TFT_DC_PIN); + SET_OUTPUT(TFT_CS_PIN); + WRITE(TFT_DC_PIN, HIGH); + WRITE(TFT_CS_PIN, HIGH); + + /** + * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz + * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1 + * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2 + */ + #if 0 + #if SPI_DEVICE == 1 + #define SPI_CLOCK_MAX SPI_CLOCK_DIV4 + #else + #define SPI_CLOCK_MAX SPI_CLOCK_DIV2 + #endif + uint8_t clock; + uint8_t spiRate = SPI_FULL_SPEED; + switch (spiRate) { + case SPI_FULL_SPEED: clock = SPI_CLOCK_MAX ; break; + case SPI_HALF_SPEED: clock = SPI_CLOCK_DIV4 ; break; + case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8 ; break; + case SPI_EIGHTH_SPEED: clock = SPI_CLOCK_DIV16; break; + case SPI_SPEED_5: clock = SPI_CLOCK_DIV32; break; + case SPI_SPEED_6: clock = SPI_CLOCK_DIV64; break; + default: clock = SPI_CLOCK_DIV2; // Default from the SPI library + } + #endif + + #if TFT_MISO_PIN == BOARD_SPI1_MISO_PIN + SPIx.setModule(1); + #elif TFT_MISO_PIN == BOARD_SPI2_MISO_PIN + SPIx.setModule(2); + #endif + SPIx.setClock(SPI_CLOCK_MAX_TFT); + SPIx.setBitOrder(MSBFIRST); + SPIx.setDataMode(SPI_MODE0); +} + +void TFT_SPI::DataTransferBegin(uint16_t DataSize) { + SPIx.setDataSize(DataSize); + SPIx.begin(); + WRITE(TFT_CS_PIN, LOW); +} + +uint32_t TFT_SPI::GetID() { + uint32_t id; + id = ReadID(LCD_READ_ID); + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = ReadID(LCD_READ_ID4); + return id; +} + +uint32_t TFT_SPI::ReadID(uint16_t Reg) { + uint32_t data = 0; + + #if PIN_EXISTS(TFT_MISO) + uint8_t d = 0; + SPIx.setDataSize(DATASIZE_8BIT); + SPIx.setClock(SPI_CLOCK_DIV64); + SPIx.begin(); + WRITE(TFT_CS_PIN, LOW); + WriteReg(Reg); + + LOOP_L_N(i, 4) { + SPIx.read((uint8_t*)&d, 1); + data = (data << 8) | d; + } + + DataTransferEnd(); + SPIx.setClock(SPI_CLOCK_MAX_TFT); + #endif + + return data >> 7; +} + +bool TFT_SPI::isBusy() { return false; } + +void TFT_SPI::Abort() { DataTransferEnd(); } + +void TFT_SPI::Transmit(uint16_t Data) { SPIx.transfer(Data); } + +void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + DataTransferBegin(DATASIZE_16BIT); + WRITE(TFT_DC_PIN, HIGH); + SPIx.dmaSend(Data, Count, MemoryIncrease); + DataTransferEnd(); +} + +#endif // HAS_SPI_TFT diff --git a/src/HAL/LPC1768/tft/tft_spi.h b/src/HAL/LPC1768/tft/tft_spi.h new file mode 100644 index 0000000..4753fdb --- /dev/null +++ b/src/HAL/LPC1768/tft/tft_spi.h @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#include +#include +// #include + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT SSP_DATABIT_8 +#define DATASIZE_16BIT SSP_DATABIT_16 +#define TFT_IO_DRIVER TFT_SPI + +#define DMA_MINC_ENABLE 1 +#define DMA_MINC_DISABLE 0 + +class TFT_SPI { +private: + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + +public: + static SPIClass SPIx; + + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATASIZE_16BIT); + static void DataTransferEnd() { OUT_WRITE(TFT_CS_PIN, HIGH); SPIx.end(); }; + static void DataTransferAbort(); + + static void WriteData(uint16_t Data) { Transmit(Data); } + static void WriteReg(uint16_t Reg) { OUT_WRITE(TFT_A0_PIN, LOW); Transmit(Reg); OUT_WRITE(TFT_A0_PIN, HIGH); } + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_MINC_ENABLE, Data, Count); } + // static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_MINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + //LPC dma can only write 0xFFF bytes at once. + #define MAX_DMA_SIZE (0xFFF - 1) + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > MAX_DMA_SIZE ? MAX_DMA_SIZE : Count); + Count = Count > MAX_DMA_SIZE ? Count - MAX_DMA_SIZE : 0; + } + #undef MAX_DMA_SIZE + } +}; diff --git a/src/HAL/LPC1768/tft/xpt2046.cpp b/src/HAL/LPC1768/tft/xpt2046.cpp new file mode 100644 index 0000000..9c1e158 --- /dev/null +++ b/src/HAL/LPC1768/tft/xpt2046.cpp @@ -0,0 +1,131 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS + +#include "xpt2046.h" +#include + +uint16_t delta(uint16_t a, uint16_t b) { return a > b ? a - b : b - a; } + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include + + SPIClass XPT2046::SPIx(TOUCH_BUTTONS_HW_SPI_DEVICE); + + static void touch_spi_init(uint8_t spiRate) { + XPT2046::SPIx.setModule(TOUCH_BUTTONS_HW_SPI_DEVICE); + XPT2046::SPIx.setClock(SPI_CLOCK_DIV128); + XPT2046::SPIx.setBitOrder(MSBFIRST); + XPT2046::SPIx.setDataMode(SPI_MODE0); + XPT2046::SPIx.setDataSize(DATA_SIZE_8BIT); + } +#endif + +void XPT2046::Init() { + SET_INPUT(TOUCH_MISO_PIN); + SET_OUTPUT(TOUCH_MOSI_PIN); + SET_OUTPUT(TOUCH_SCK_PIN); + OUT_WRITE(TOUCH_CS_PIN, HIGH); + + #if PIN_EXISTS(TOUCH_INT) + // Optional Pendrive interrupt pin + SET_INPUT(TOUCH_INT_PIN); + #endif + + TERN_(TOUCH_BUTTONS_HW_SPI, touch_spi_init(SPI_SPEED_6)); + + // Read once to enable pendrive status pin + getRawData(XPT2046_X); +} + +bool XPT2046::isTouched() { + return isBusy() ? false : ( + #if PIN_EXISTS(TOUCH_INT) + READ(TOUCH_INT_PIN) != HIGH + #else + getRawData(XPT2046_Z1) >= XPT2046_Z1_THRESHOLD + #endif + ); +} + +bool XPT2046::getRawPoint(int16_t *x, int16_t *y) { + if (isBusy()) return false; + if (!isTouched()) return false; + *x = getRawData(XPT2046_X); + *y = getRawData(XPT2046_Y); + return isTouched(); +} + +uint16_t XPT2046::getRawData(const XPTCoordinate coordinate) { + uint16_t data[3]; + + DataTransferBegin(); + TERN_(TOUCH_BUTTONS_HW_SPI, SPIx.begin()); + + for (uint16_t i = 0; i < 3 ; i++) { + IO(coordinate); + data[i] = (IO() << 4) | (IO() >> 4); + } + + TERN_(TOUCH_BUTTONS_HW_SPI, SPIx.end()); + DataTransferEnd(); + + uint16_t delta01 = delta(data[0], data[1]), + delta02 = delta(data[0], data[2]), + delta12 = delta(data[1], data[2]); + + if (delta01 > delta02 || delta01 > delta12) + data[delta02 > delta12 ? 0 : 1] = data[2]; + + return (data[0] + data[1]) >> 1; +} + +uint16_t XPT2046::IO(uint16_t data) { + return TERN(TOUCH_BUTTONS_HW_SPI, HardwareIO, SoftwareIO)(data); +} + +extern uint8_t spiTransfer(uint8_t b); + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + uint16_t XPT2046::HardwareIO(uint16_t data) { + return SPIx.transfer(data & 0xFF); + } +#endif + +uint16_t XPT2046::SoftwareIO(uint16_t data) { + uint16_t result = 0; + + for (uint8_t j = 0x80; j; j >>= 1) { + WRITE(TOUCH_SCK_PIN, LOW); + WRITE(TOUCH_MOSI_PIN, data & j ? HIGH : LOW); + if (READ(TOUCH_MISO_PIN)) result |= j; + WRITE(TOUCH_SCK_PIN, HIGH); + } + WRITE(TOUCH_SCK_PIN, LOW); + + return result; +} + +#endif // HAS_TFT_XPT2046 diff --git a/src/HAL/LPC1768/tft/xpt2046.h b/src/HAL/LPC1768/tft/xpt2046.h new file mode 100644 index 0000000..7c456cf --- /dev/null +++ b/src/HAL/LPC1768/tft/xpt2046.h @@ -0,0 +1,83 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include +#endif + +#ifndef TOUCH_MISO_PIN + #define TOUCH_MISO_PIN SD_MISO_PIN +#endif +#ifndef TOUCH_MOSI_PIN + #define TOUCH_MOSI_PIN SD_MOSI_PIN +#endif +#ifndef TOUCH_SCK_PIN + #define TOUCH_SCK_PIN SD_SCK_PIN +#endif +#ifndef TOUCH_CS_PIN + #define TOUCH_CS_PIN SD_SS_PIN +#endif +#ifndef TOUCH_INT_PIN + #define TOUCH_INT_PIN -1 +#endif + +#define XPT2046_DFR_MODE 0x00 +#define XPT2046_SER_MODE 0x04 +#define XPT2046_CONTROL 0x80 + +enum XPTCoordinate : uint8_t { + XPT2046_X = 0x10 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Y = 0x50 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z1 = 0x30 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, +}; + +#ifndef XPT2046_Z1_THRESHOLD + #define XPT2046_Z1_THRESHOLD 10 +#endif + +class XPT2046 { +private: + static bool isBusy() { return false; } + + static uint16_t getRawData(const XPTCoordinate coordinate); + static bool isTouched(); + + static void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static uint16_t HardwareIO(uint16_t data); + #endif + static uint16_t SoftwareIO(uint16_t data); + static uint16_t IO(uint16_t data = 0); + +public: + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static SPIClass SPIx; + #endif + + static void Init(); + static bool getRawPoint(int16_t *x, int16_t *y); +}; diff --git a/src/HAL/LPC1768/timers.cpp b/src/HAL/LPC1768/timers.cpp new file mode 100644 index 0000000..bbb13f8 --- /dev/null +++ b/src/HAL/LPC1768/timers.cpp @@ -0,0 +1,65 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Description: + * + * Timers for LPC1768 + */ + +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfig.h" + +void HAL_timer_init() { + SBI(LPC_SC->PCONP, SBIT_TIMER0); // Power ON Timer 0 + LPC_TIM0->PR = (HAL_TIMER_RATE) / (STEPPER_TIMER_RATE) - 1; // Use prescaler to set frequency if needed + + SBI(LPC_SC->PCONP, SBIT_TIMER1); // Power ON Timer 1 + LPC_TIM1->PR = (HAL_TIMER_RATE) / 1000000 - 1; +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + switch (timer_num) { + case MF_TIMER_STEP: + LPC_TIM0->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them + LPC_TIM0->MR0 = uint32_t(STEPPER_TIMER_RATE) / frequency; // Match value (period) to set frequency + LPC_TIM0->TCR = _BV(SBIT_CNTEN); // Counter Enable + + NVIC_SetPriority(TIMER0_IRQn, NVIC_EncodePriority(0, 1, 0)); + NVIC_EnableIRQ(TIMER0_IRQn); + break; + + case MF_TIMER_TEMP: + LPC_TIM1->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them + LPC_TIM1->MR0 = uint32_t(TEMP_TIMER_RATE) / frequency; + LPC_TIM1->TCR = _BV(SBIT_CNTEN); // Counter Enable + + NVIC_SetPriority(TIMER1_IRQn, NVIC_EncodePriority(0, 2, 0)); + NVIC_EnableIRQ(TIMER1_IRQn); + break; + + default: break; + } +} + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/timers.h b/src/HAL/LPC1768/timers.h new file mode 100644 index 0000000..c6d7bc6 --- /dev/null +++ b/src/HAL/LPC1768/timers.h @@ -0,0 +1,173 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL For LPC1768 + */ + +#include + +#include "../../core/macros.h" + +#define SBIT_TIMER0 1 +#define SBIT_TIMER1 2 + +#define SBIT_CNTEN 0 + +#define SBIT_MR0I 0 // Timer 0 Interrupt when TC matches MR0 +#define SBIT_MR0R 1 // Timer 0 Reset TC on Match +#define SBIT_MR0S 2 // Timer 0 Stop TC and PC on Match +#define SBIT_MR1I 3 +#define SBIT_MR1R 4 +#define SBIT_MR1S 5 +#define SBIT_MR2I 6 +#define SBIT_MR2R 7 +#define SBIT_MR2S 8 +#define SBIT_MR3I 9 +#define SBIT_MR3R 10 +#define SBIT_MR3S 11 + +// ------------------------ +// Defines +// ------------------------ + +#define _HAL_TIMER(T) _CAT(LPC_TIM, T) +#define _HAL_TIMER_IRQ(T) TIMER##T##_IRQn +#define __HAL_TIMER_ISR(T) extern "C" void TIMER##T##_IRQHandler() +#define _HAL_TIMER_ISR(T) __HAL_TIMER_ISR(T) + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define HAL_TIMER_RATE ((F_CPU) / 4) // frequency of timers peripherals + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif +#ifndef MF_TIMER_PWM + #define MF_TIMER_PWM 3 // Timer Index for PWM +#endif + +#define TEMP_TIMER_RATE 1000000 +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() _HAL_TIMER_ISR(MF_TIMER_STEP) +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() _HAL_TIMER_ISR(MF_TIMER_TEMP) +#endif + +// Timer references by index +#define STEP_TIMER_PTR _HAL_TIMER(MF_TIMER_STEP) +#define TEMP_TIMER_PTR _HAL_TIMER(MF_TIMER_TEMP) + +// ------------------------ +// Public functions +// ------------------------ +void HAL_timer_init(); +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + switch (timer_num) { + case MF_TIMER_STEP: STEP_TIMER_PTR->MR0 = compare; break; // Stepper Timer Match Register 0 + case MF_TIMER_TEMP: TEMP_TIMER_PTR->MR0 = compare; break; // Temp Timer Match Register 0 + } +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return STEP_TIMER_PTR->MR0; // Stepper Timer Match Register 0 + case MF_TIMER_TEMP: return TEMP_TIMER_PTR->MR0; // Temp Timer Match Register 0 + } + return 0; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return STEP_TIMER_PTR->TC; // Stepper Timer Count + case MF_TIMER_TEMP: return TEMP_TIMER_PTR->TC; // Temp Timer Count + } + return 0; +} + +FORCE_INLINE static void HAL_timer_enable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_EnableIRQ(TIMER0_IRQn); break; // Enable interrupt handler + case MF_TIMER_TEMP: NVIC_EnableIRQ(TIMER1_IRQn); break; // Enable interrupt handler + } +} + +FORCE_INLINE static void HAL_timer_disable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_DisableIRQ(TIMER0_IRQn); break; // Disable interrupt handler + case MF_TIMER_TEMP: NVIC_DisableIRQ(TIMER1_IRQn); break; // Disable interrupt handler + } + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); +} + +// This function is missing from CMSIS +FORCE_INLINE static bool NVIC_GetEnableIRQ(IRQn_Type IRQn) { + return TEST(NVIC->ISER[uint32_t(IRQn) >> 5], uint32_t(IRQn) & 0x1F); +} + +FORCE_INLINE static bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return NVIC_GetEnableIRQ(TIMER0_IRQn); // Check if interrupt is enabled or not + case MF_TIMER_TEMP: return NVIC_GetEnableIRQ(TIMER1_IRQn); // Check if interrupt is enabled or not + } + return false; +} + +FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: SBI(STEP_TIMER_PTR->IR, SBIT_CNTEN); break; + case MF_TIMER_TEMP: SBI(TEMP_TIMER_PTR->IR, SBIT_CNTEN); break; + } +} + +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/LPC1768/u8g/LCD_I2C_routines.cpp b/src/HAL/LPC1768/u8g/LCD_I2C_routines.cpp new file mode 100644 index 0000000..e714c3c --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_I2C_routines.cpp @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +// adapted from I2C/master/master.c example +// https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html + +#ifdef TARGET_LPC1768 + +#include "../include/i2c_util.h" +#include "../../../core/millis_t.h" + +extern int millis(); + +#ifdef __cplusplus + extern "C" { +#endif + +////////////////////////////////////////////////////////////////////////////////////// + +#define I2CDEV_S_ADDR 0x78 // From SSD1306 (actual address is 0x3C - shift left 1 with LSB set to 0 to indicate write) + +// Send slave address and write bit +uint8_t u8g_i2c_start(const uint8_t sla) { + // Sometimes TX data ACK or NAK status is returned. That mean the start state didn't + // happen which means only the value of the slave address was send. Keep looping until + // the slave address and write bit are actually sent. + do{ + _I2C_Stop(I2CDEV_M); // output stop state on I2C bus + _I2C_Start(I2CDEV_M); // output start state on I2C bus + while ((I2C_status != I2C_I2STAT_M_TX_START) + && (I2C_status != I2C_I2STAT_M_TX_RESTART) + && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK)); //wait for start to be asserted + + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_STAC; // clear start state before tansmitting slave address + LPC_I2C1->I2DAT = I2CDEV_S_ADDR & I2C_I2DAT_BITMASK; // transmit slave address & write bit + LPC_I2C1->I2CONSET = I2C_I2CONSET_AA; + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC; + while ((I2C_status != I2C_I2STAT_M_TX_SLAW_ACK) + && (I2C_status != I2C_I2STAT_M_TX_SLAW_NACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_ACK) + && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK)); //wait for slaw to finish + }while ( (I2C_status == I2C_I2STAT_M_TX_DAT_ACK) || (I2C_status == I2C_I2STAT_M_TX_DAT_NACK)); + return 1; +} + +void u8g_i2c_init(const uint8_t clock_option) { + configure_i2c(clock_option); + u8g_i2c_start(0); // Send slave address and write bit +} + +uint8_t u8g_i2c_send_byte(uint8_t data) { + #define I2C_TIMEOUT 3 + LPC_I2C1->I2DAT = data & I2C_I2DAT_BITMASK; // transmit data + LPC_I2C1->I2CONSET = I2C_I2CONSET_AA; + LPC_I2C1->I2CONCLR = I2C_I2CONCLR_SIC; + const millis_t timeout = millis() + I2C_TIMEOUT; + while ((I2C_status != I2C_I2STAT_M_TX_DAT_ACK) && (I2C_status != I2C_I2STAT_M_TX_DAT_NACK) && PENDING(millis(), timeout)); // wait for xmit to finish + // had hangs with SH1106 so added time out - have seen temporary screen corruption when this happens + return 1; +} + +void u8g_i2c_stop() { +} + +#ifdef __cplusplus + } +#endif + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/LCD_I2C_routines.h b/src/HAL/LPC1768/u8g/LCD_I2C_routines.h new file mode 100644 index 0000000..2d976c9 --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_I2C_routines.h @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void u8g_i2c_init(const uint8_t clock_options); +//uint8_t u8g_i2c_wait(uint8_t mask, uint8_t pos); +uint8_t u8g_i2c_start(uint8_t sla); +uint8_t u8g_i2c_send_byte(uint8_t data); +void u8g_i2c_stop(); diff --git a/src/HAL/LPC1768/u8g/LCD_defines.h b/src/HAL/LPC1768/u8g/LCD_defines.h new file mode 100644 index 0000000..d226003 --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_defines.h @@ -0,0 +1,49 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * LPC1768 LCD-specific defines + */ + +// The following are optional depending on the platform. + +// definitions of HAL specific com and device drivers. +uint8_t u8g_com_HAL_LPC1768_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_HAL_LPC1768_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_HAL_LPC1768_ST7920_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_HAL_LPC1768_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_HAL_LPC1768_ssd_hw_i2c_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); + + +// connect U8g com generic com names to the desired driver +#define U8G_COM_HW_SPI u8g_com_HAL_LPC1768_hw_spi_fn // use LPC1768 specific hardware SPI routine +#define U8G_COM_SW_SPI u8g_com_HAL_LPC1768_sw_spi_fn // use LPC1768 specific software SPI routine +#define U8G_COM_ST7920_HW_SPI u8g_com_HAL_LPC1768_ST7920_hw_spi_fn +#define U8G_COM_ST7920_SW_SPI u8g_com_HAL_LPC1768_ST7920_sw_spi_fn +#define U8G_COM_SSD_I2C u8g_com_HAL_LPC1768_ssd_hw_i2c_fn + +// let these default for now +#define U8G_COM_PARALLEL u8g_com_null_fn +#define U8G_COM_T6963 u8g_com_null_fn +#define U8G_COM_FAST_PARALLEL u8g_com_null_fn +#define U8G_COM_UC_I2C u8g_com_null_fn diff --git a/src/HAL/LPC1768/u8g/LCD_delay.h b/src/HAL/LPC1768/u8g/LCD_delay.h new file mode 100644 index 0000000..0b9e2b4 --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_delay.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * LCD delay routines - used by all the drivers. + * + * These are based on the LPC1768 routines. + * + * Couldn't just call exact copies because the overhead + * results in a one microsecond delay taking about 4µS. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +void U8g_delay(int msec); +void u8g_MicroDelay(); +void u8g_10MicroDelay(); + +#ifdef __cplusplus + } +#endif diff --git a/src/HAL/LPC1768/u8g/LCD_pin_routines.c b/src/HAL/LPC1768/u8g/LCD_pin_routines.c new file mode 100644 index 0000000..466fc80 --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_pin_routines.c @@ -0,0 +1,110 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + +#ifdef TARGET_LPC1768 + +#include +#include +#include "../../../core/macros.h" +//#include + +#define LPC_PORT_OFFSET (0x0020) +#define LPC_PIN(pin) (1UL << pin) +#define LPC_GPIO(port) ((volatile LPC_GPIO_TypeDef *)(LPC_GPIO0_BASE + LPC_PORT_OFFSET * port)) + +#define INPUT 0 +#define OUTPUT 1 +#define INPUT_PULLUP 2 + +uint8_t LPC1768_PIN_PORT(const uint8_t pin); +uint8_t LPC1768_PIN_PIN(const uint8_t pin); + +#ifdef __cplusplus + extern "C" { +#endif + +// I/O functions +// As defined by Arduino INPUT(0x0), OUTPUT(0x1), INPUT_PULLUP(0x2) +void pinMode_LCD(uint8_t pin, uint8_t mode) { + #define LPC1768_PIN_PORT(pin) ((uint8_t)((pin >> 5) & 0b111)) + #define LPC1768_PIN_PIN(pin) ((uint8_t)(pin & 0b11111)) + PINSEL_CFG_Type config = { LPC1768_PIN_PORT(pin), + LPC1768_PIN_PIN(pin), + PINSEL_FUNC_0, + PINSEL_PINMODE_TRISTATE, + PINSEL_PINMODE_NORMAL }; + switch (mode) { + case INPUT: + LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(pin)); + PINSEL_ConfigPin(&config); + break; + case OUTPUT: + LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR |= LPC_PIN(LPC1768_PIN_PIN(pin)); + PINSEL_ConfigPin(&config); + break; + case INPUT_PULLUP: + LPC_GPIO(LPC1768_PIN_PORT(pin))->FIODIR &= ~LPC_PIN(LPC1768_PIN_PIN(pin)); + config.Pinmode = PINSEL_PINMODE_PULLUP; + PINSEL_ConfigPin(&config); + break; + default: break; + } +} + +void u8g_SetPinOutput(uint8_t internal_pin_number) { + pinMode_LCD(internal_pin_number, 1); // OUTPUT +} + +void u8g_SetPinInput(uint8_t internal_pin_number) { + pinMode_LCD(internal_pin_number, 0); // INPUT +} + +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status) { + #define LPC1768_PIN_PORT(pin) ((uint8_t)((pin >> 5) & 0b111)) + #define LPC1768_PIN_PIN(pin) ((uint8_t)(pin & 0b11111)) + if (pin_status) + LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOSET = LPC_PIN(LPC1768_PIN_PIN(pin)); + else + LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOCLR = LPC_PIN(LPC1768_PIN_PIN(pin)); +} + +uint8_t u8g_GetPinLevel(uint8_t pin) { + #define LPC1768_PIN_PORT(pin) ((uint8_t)((pin >> 5) & 0b111)) + #define LPC1768_PIN_PIN(pin) ((uint8_t)(pin & 0b11111)) + return (uint32_t)LPC_GPIO(LPC1768_PIN_PORT(pin))->FIOPIN & LPC_PIN(LPC1768_PIN_PIN(pin)) ? 1 : 0; +} + +#ifdef __cplusplus + } +#endif + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/LCD_pin_routines.h b/src/HAL/LPC1768/u8g/LCD_pin_routines.h new file mode 100644 index 0000000..d60d93d --- /dev/null +++ b/src/HAL/LPC1768/u8g/LCD_pin_routines.h @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + +void u8g_SetPinOutput(uint8_t internal_pin_number); +void u8g_SetPinInput(uint8_t internal_pin_number); +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status); +uint8_t u8g_GetPinLevel(uint8_t pin); diff --git a/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp new file mode 100644 index 0000000..0118f92 --- /dev/null +++ b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp @@ -0,0 +1,129 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_msp430_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include +#include "../../shared/HAL_SPI.h" + +#ifndef LCD_SPI_SPEED + #ifdef SD_SPI_SPEED + #define LCD_SPI_SPEED SD_SPI_SPEED // Assume SPI speed shared with SD + #else + #define LCD_SPI_SPEED SPI_FULL_SPEED // Use full speed if SD speed is not supplied + #endif +#endif + +uint8_t u8g_com_HAL_LPC1768_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_INIT: + u8g_SetPILevel(u8g, U8G_PI_CS, 1); + u8g_SetPILevel(u8g, U8G_PI_A0, 1); + u8g_SetPILevel(u8g, U8G_PI_RESET, 1); + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_A0); + u8g_SetPIOutput(u8g, U8G_PI_RESET); + u8g_Delay(5); + spiBegin(); + spiInit(LCD_SPI_SPEED); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel(u8g, U8G_PI_A0, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + u8g_SetPILevel(u8g, U8G_PI_CS, (arg_val ? 0 : 1)); + break; + + case U8G_COM_MSG_RESET: + u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_WRITE_BYTE: + spiSend((uint8_t)arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + } + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp new file mode 100644 index 0000000..bf76eaf --- /dev/null +++ b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp @@ -0,0 +1,198 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_arduino_ssd_i2c.c + * + * COM interface for Arduino (AND ATmega) and the SSDxxxx chip (SOLOMON) variant + * I2C protocol + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/** + * Special pin usage: + * U8G_PI_I2C_OPTION additional options + * U8G_PI_A0_STATE used to store the last value of the command/data register selection + * U8G_PI_SET_A0 1: Signal request to update I2C device with new A0_STATE, 0: Do nothing, A0_STATE matches I2C device + * U8G_PI_SCL clock line (NOT USED) + * U8G_PI_SDA data line (NOT USED) + * + * U8G_PI_RESET reset line (currently disabled, see below) + * + * Protocol: + * SLA, Cmd/Data Selection, Arguments + * The command/data register is selected by a special instruction byte, which is sent after SLA + * + * The continue bit is always 0 so that a (re)start is equired for the change from cmd to/data mode + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include + +#define I2C_SLA (0x3C*2) +//#define I2C_CMD_MODE 0x080 +#define I2C_CMD_MODE 0x000 +#define I2C_DATA_MODE 0x040 + +uint8_t u8g_com_ssd_I2C_start_sequence(u8g_t *u8g) { + /* are we requested to set the a0 state? */ + if (u8g->pin_list[U8G_PI_SET_A0] == 0) return 1; + + /* setup bus, might be a repeated start */ + if (u8g_i2c_start(I2C_SLA) == 0) return 0; + if (u8g->pin_list[U8G_PI_A0_STATE] == 0) { + if (u8g_i2c_send_byte(I2C_CMD_MODE) == 0) return 0; + } + else if (u8g_i2c_send_byte(I2C_DATA_MODE) == 0) + return 0; + + u8g->pin_list[U8G_PI_SET_A0] = 0; + return 1; +} + +uint8_t u8g_com_HAL_LPC1768_ssd_hw_i2c_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + //u8g_com_arduino_digital_write(u8g, U8G_PI_SCL, HIGH); + //u8g_com_arduino_digital_write(u8g, U8G_PI_SDA, HIGH); + //u8g->pin_list[U8G_PI_A0_STATE] = 0; /* initial RS state: unknown mode */ + + u8g_i2c_init(u8g->pin_list[U8G_PI_I2C_OPTION]); + u8g_com_ssd_I2C_start_sequence(u8g); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + /* Currently disabled, but it could be enable. Previous restrictions have been removed */ + /* u8g_com_arduino_digital_write(u8g, U8G_PI_RESET, arg_val); */ + break; + + case U8G_COM_MSG_CHIP_SELECT: + u8g->pin_list[U8G_PI_A0_STATE] = 0; + u8g->pin_list[U8G_PI_SET_A0] = 1; /* force a0 to set again, also forces start condition */ + if (arg_val == 0 ) { + /* disable chip, send stop condition */ + u8g_i2c_stop(); + } + else { + /* enable, do nothing: any byte writing will trigger the i2c start */ + } + break; + + case U8G_COM_MSG_WRITE_BYTE: + //u8g->pin_list[U8G_PI_SET_A0] = 1; + if (u8g_com_ssd_I2C_start_sequence(u8g) == 0) { + u8g_i2c_stop(); + return 0; + } + + if (u8g_i2c_send_byte(arg_val) == 0) { + u8g_i2c_stop(); + return 0; + } + // u8g_i2c_stop(); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + //u8g->pin_list[U8G_PI_SET_A0] = 1; + if (u8g_com_ssd_I2C_start_sequence(u8g) == 0) { + u8g_i2c_stop(); + return 0; + } + + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + if (u8g_i2c_send_byte(*ptr++) == 0) { + u8g_i2c_stop(); + return 0; + } + arg_val--; + } + } + // u8g_i2c_stop(); + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + //u8g->pin_list[U8G_PI_SET_A0] = 1; + if (u8g_com_ssd_I2C_start_sequence(u8g) == 0) { + u8g_i2c_stop(); + return 0; + } + + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + if (u8g_i2c_send_byte(u8g_pgm_read(ptr)) == 0) + return 0; + ptr++; + arg_val--; + } + } + // u8g_i2c_stop(); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + u8g->pin_list[U8G_PI_SET_A0] = 1; /* force a0 to set again */ + break; + + } // switch + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp new file mode 100644 index 0000000..ce7b338 --- /dev/null +++ b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp @@ -0,0 +1,138 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_LPC1768_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB + +#include +#include "../../shared/HAL_SPI.h" +#include "../../shared/Delay.h" + +void spiBegin(); +void spiInit(uint8_t spiRate); +void spiSend(uint8_t b); +void spiSend(const uint8_t *buf, size_t n); + +static uint8_t rs_last_state = 255; + +static void u8g_com_LPC1768_st7920_write_byte_hw_spi(uint8_t rs, uint8_t val) { + + if (rs != rs_last_state) { // Time to send a command/data byte + rs_last_state = rs; + spiSend(rs ? 0x0FA : 0x0F8); // Send data or command + DELAY_US(40); // Give the controller some time: 20 is bad, 30 is OK, 40 is safe + } + + spiSend(val & 0xF0); + spiSend(val << 4); +} + +uint8_t u8g_com_HAL_LPC1768_ST7920_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + u8g_SetPILevel(u8g, U8G_PI_CS, 0); + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_Delay(5); + spiBegin(); + spiInit(SPI_EIGHTH_SPEED); // ST7920 max speed is about 1.1 MHz + u8g->pin_list[U8G_PI_A0_STATE] = 0; // initial RS state: command mode + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_ADDRESS: // Define cmd (arg_val = 0) or data mode (arg_val = 1) + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + break; + + case U8G_COM_MSG_CHIP_SELECT: + u8g_SetPILevel(u8g, U8G_PI_CS, arg_val); // Note: the ST7920 has an active high chip-select + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_com_LPC1768_st7920_write_byte_hw_spi(u8g->pin_list[U8G_PI_A0_STATE], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_LPC1768_st7920_write_byte_hw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_LPC1768_st7920_write_byte_hw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB + +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp new file mode 100644 index 0000000..e159eba --- /dev/null +++ b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp @@ -0,0 +1,147 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if IS_U8GLIB_ST7920 + +#include +#include +#include "../../shared/Delay.h" +#include "../../shared/HAL_SPI.h" + +#ifndef LCD_SPI_SPEED + #define LCD_SPI_SPEED SPI_EIGHTH_SPEED // About 1 MHz +#endif + +static pin_t SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL; +static uint8_t SPI_speed = 0; + +static void u8g_com_LPC1768_st7920_write_byte_sw_spi(uint8_t rs, uint8_t val) { + static uint8_t rs_last_state = 255; + if (rs != rs_last_state) { + // Transfer Data (FA) or Command (F8) + swSpiTransfer(rs ? 0x0FA : 0x0F8, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + rs_last_state = rs; + DELAY_US(40); // Give the controller time to process the data: 20 is bad, 30 is OK, 40 is safe + } + swSpiTransfer(val & 0x0F0, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + swSpiTransfer(val << 4, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); +} + +uint8_t u8g_com_HAL_LPC1768_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SCK_pin_ST7920_HAL = u8g->pin_list[U8G_PI_SCK]; + MOSI_pin_ST7920_HAL_HAL = u8g->pin_list[U8G_PI_MOSI]; + + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_Delay(5); + + SPI_speed = swSpiInit(LCD_SPI_SPEED, SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL); + + u8g_SetPILevel(u8g, U8G_PI_CS, 0); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + + u8g->pin_list[U8G_PI_A0_STATE] = 0; /* initial RS state: command mode */ + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + break; + + case U8G_COM_MSG_CHIP_SELECT: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_CS]) u8g_SetPILevel(u8g, U8G_PI_CS, arg_val); //note: the st7920 has an active high chip select + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_com_LPC1768_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_LPC1768_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_LPC1768_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + } + return 1; +} + +#endif // IS_U8GLIB_ST7920 +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp new file mode 100644 index 0000000..f116a9b --- /dev/null +++ b/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp @@ -0,0 +1,209 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_std_sw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2015, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef TARGET_LPC1768 + +#include "../../../inc/MarlinConfigPre.h" + +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#include +#include "../../shared/HAL_SPI.h" + +#ifndef LCD_SPI_SPEED + #define LCD_SPI_SPEED SPI_QUARTER_SPEED // About 2 MHz +#endif + +#include +#include +#include +#include + +#include + +uint8_t swSpiTransfer_mode_0(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + + LOOP_L_N(i, 8) { + if (spi_speed == 0) { + LPC176x::gpio_set(mosi_pin, !!(b & 0x80)); + LPC176x::gpio_set(sck_pin, HIGH); + b <<= 1; + if (miso_pin >= 0 && LPC176x::gpio_get(miso_pin)) b |= 1; + LPC176x::gpio_set(sck_pin, LOW); + } + else { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + LOOP_L_N(j, spi_speed) + LPC176x::gpio_set(mosi_pin, state); + + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + LPC176x::gpio_set(sck_pin, HIGH); + + b <<= 1; + if (miso_pin >= 0 && LPC176x::gpio_get(miso_pin)) b |= 1; + + LOOP_L_N(j, spi_speed) + LPC176x::gpio_set(sck_pin, LOW); + } + } + + return b; +} + +uint8_t swSpiTransfer_mode_3(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + + LOOP_L_N(i, 8) { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + if (spi_speed == 0) { + LPC176x::gpio_set(sck_pin, LOW); + LPC176x::gpio_set(mosi_pin, state); + LPC176x::gpio_set(mosi_pin, state); // need some setup time + LPC176x::gpio_set(sck_pin, HIGH); + } + else { + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + LPC176x::gpio_set(sck_pin, LOW); + + LOOP_L_N(j, spi_speed) + LPC176x::gpio_set(mosi_pin, state); + + LOOP_L_N(j, spi_speed) + LPC176x::gpio_set(sck_pin, HIGH); + } + b <<= 1; + if (miso_pin >= 0 && LPC176x::gpio_get(miso_pin)) b |= 1; + } + + return b; +} + +static uint8_t SPI_speed = 0; + +static void u8g_sw_spi_HAL_LPC1768_shift_out(uint8_t dataPin, uint8_t clockPin, uint8_t val) { + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) + swSpiTransfer_mode_3(val, SPI_speed, clockPin, -1, dataPin); + #else + swSpiTransfer_mode_0(val, SPI_speed, clockPin, -1, dataPin); + #endif +} + +uint8_t u8g_com_HAL_LPC1768_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_A0); + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPIOutput(u8g, U8G_PI_RESET); + SPI_speed = swSpiInit(LCD_SPI_SPEED, u8g->pin_list[U8G_PI_SCK], u8g->pin_list[U8G_PI_MOSI]); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) // LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + u8g_SetPILevel(u8g, U8G_PI_SCK, 1); // Set SCK to mode 3 idle state before CS goes active + u8g_SetPILevel(u8g, U8G_PI_CS, LOW); + } + else { + u8g_SetPILevel(u8g, U8G_PI_CS, HIGH); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); // Set SCK to mode 0 idle state after CS goes inactive + } + #else + u8g_SetPILevel(u8g, U8G_PI_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_sw_spi_HAL_LPC1768_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_HAL_LPC1768_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_HAL_LPC1768_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel(u8g, U8G_PI_A0, arg_val); + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/upload_extra_script.py b/src/HAL/LPC1768/upload_extra_script.py new file mode 100644 index 0000000..7975f15 --- /dev/null +++ b/src/HAL/LPC1768/upload_extra_script.py @@ -0,0 +1,127 @@ +# +# upload_extra_script.py +# set the output_port +# if target_filename is found then that drive is used +# else if target_drive is found then that drive is used +# +from __future__ import print_function + +import pioutil +if pioutil.is_pio_build(): + + target_filename = "FIRMWARE.CUR" + target_drive = "REARM" + + import os,getpass,platform + + current_OS = platform.system() + Import("env") + + def print_error(e): + print('\nUnable to find destination disk (%s)\n' \ + 'Please select it in platformio.ini using the upload_port keyword ' \ + '(https://docs.platformio.org/en/latest/projectconf/section_env_upload.html) ' \ + 'or copy the firmware (.pio/build/%s/firmware.bin) manually to the appropriate disk\n' \ + %(e, env.get('PIOENV'))) + + def before_upload(source, target, env): + try: + # + # Find a disk for upload + # + upload_disk = 'Disk not found' + target_file_found = False + target_drive_found = False + if current_OS == 'Windows': + # + # platformio.ini will accept this for a Windows upload port designation: 'upload_port = L:' + # Windows - doesn't care about the disk's name, only cares about the drive letter + import subprocess,string + from ctypes import windll + + # getting list of drives + # https://stackoverflow.com/questions/827371/is-there-a-way-to-list-all-the-available-drive-letters-in-python + drives = [] + bitmask = windll.kernel32.GetLogicalDrives() + for letter in string.ascii_uppercase: + if bitmask & 1: + drives.append(letter) + bitmask >>= 1 + + for drive in drives: + final_drive_name = drive + ':\\' + # print ('disc check: {}'.format(final_drive_name)) + try: + volume_info = str(subprocess.check_output('cmd /C dir ' + final_drive_name, stderr=subprocess.STDOUT)) + except Exception as e: + print ('error:{}'.format(e)) + continue + else: + if target_drive in volume_info and not target_file_found: # set upload if not found target file yet + target_drive_found = True + upload_disk = final_drive_name + if target_filename in volume_info: + if not target_file_found: + upload_disk = final_drive_name + target_file_found = True + + elif current_OS == 'Linux': + # + # platformio.ini will accept this for a Linux upload port designation: 'upload_port = /media/media_name/drive' + # + drives = os.listdir(os.path.join(os.sep, 'media', getpass.getuser())) + if target_drive in drives: # If target drive is found, use it. + target_drive_found = True + upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), target_drive) + os.sep + else: + for drive in drives: + try: + files = os.listdir(os.path.join(os.sep, 'media', getpass.getuser(), drive)) + except: + continue + else: + if target_filename in files: + upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), drive) + os.sep + target_file_found = True + break + # + # set upload_port to drive if found + # + + if target_file_found or target_drive_found: + env.Replace( + UPLOAD_FLAGS="-P$UPLOAD_PORT" + ) + + elif current_OS == 'Darwin': # MAC + # + # platformio.ini will accept this for a OSX upload port designation: 'upload_port = /media/media_name/drive' + # + drives = os.listdir('/Volumes') # human readable names + if target_drive in drives and not target_file_found: # set upload if not found target file yet + target_drive_found = True + upload_disk = '/Volumes/' + target_drive + '/' + for drive in drives: + try: + filenames = os.listdir('/Volumes/' + drive + '/') # will get an error if the drive is protected + except: + continue + else: + if target_filename in filenames: + if not target_file_found: + upload_disk = '/Volumes/' + drive + '/' + target_file_found = True + + # + # Set upload_port to drive if found + # + if target_file_found or target_drive_found: + env.Replace(UPLOAD_PORT=upload_disk) + print('\nUpload disk: ', upload_disk, '\n') + else: + print_error('Autodetect Error') + + except Exception as e: + print_error(str(e)) + + env.AddPreAction("upload", before_upload) diff --git a/src/HAL/LPC1768/usb_serial.cpp b/src/HAL/LPC1768/usb_serial.cpp new file mode 100644 index 0000000..3c1fce5 --- /dev/null +++ b/src/HAL/LPC1768/usb_serial.cpp @@ -0,0 +1,38 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef TARGET_LPC1768 + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(EMERGENCY_PARSER) + +#include "../../feature/e_parser.h" + +EmergencyParser::State emergency_state; + +bool CDC_RecvCallback(const char c) { + emergency_parser.update(emergency_state, c); + return true; +} + +#endif // EMERGENCY_PARSER +#endif // TARGET_LPC1768 diff --git a/src/HAL/LPC1768/win_usb_driver/lpc176x_usb_driver.inf b/src/HAL/LPC1768/win_usb_driver/lpc176x_usb_driver.inf new file mode 100644 index 0000000..9727d7d --- /dev/null +++ b/src/HAL/LPC1768/win_usb_driver/lpc176x_usb_driver.inf @@ -0,0 +1,36 @@ +[Version] +Signature="$Windows NT$" +Class=Ports +ClassGuid={4D36E978-E325-11CE-BFC1-08002BE10318} +Provider=%PROVIDER% +DriverVer =04/14/2008, 5.1.2600.5512 + +[Manufacturer] +%PROVIDER%=DeviceList,ntamd64 + + +[DeviceList] +%DESCRIPTION%=LPC1768USB, USB\VID_1D50&PID_6029&MI_00 + +[DeviceList.ntamd64] +%DESCRIPTION%=LPC1768USB, USB\VID_1D50&PID_6029&MI_00 + + +[LPC1768USB] +include=mdmcpq.inf +CopyFiles=FakeModemCopyFileSection +AddReg=LowerFilterAddReg,SerialPropPageAddReg + +[LPC1768USB.Services] +include=mdmcpq.inf +AddService=usbser, 0x00000002, LowerFilter_Service_Inst + +[SerialPropPageAddReg] +HKR,,EnumPropPages32,,"MsPorts.dll,SerialPortPropPageProvider" + + +[Strings] +PROVIDER = "marlinfw.org" +DRIVER.SVC = "Marlin USB Driver" +DESCRIPTION= "Marlin USB Serial" +COMPOSITE = "Marlin USB VCOM" \ No newline at end of file diff --git a/src/HAL/NATIVE_SIM/HAL.h b/src/HAL/NATIVE_SIM/HAL.h new file mode 100644 index 0000000..6620361 --- /dev/null +++ b/src/HAL/NATIVE_SIM/HAL.h @@ -0,0 +1,266 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include +#undef min +#undef max +#include +#include "pinmapping.h" + +void _printf (const char *format, ...); +void _putc(uint8_t c); +uint8_t _getc(); + +//arduino: Print.h +#define DEC 10 +#define HEX 16 +#define OCT 8 +#define BIN 2 +//arduino: binary.h (weird defines) +#define B01 1 +#define B10 2 + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "serial.h" + +// ------------------------ +// Defines +// ------------------------ + +#define CPU_32_BIT +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +#define F_CPU 100000000 +#define SystemCoreClock F_CPU + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +// ------------------------ +// Serial ports +// ------------------------ + +extern MSerialT serial_stream_0; +extern MSerialT serial_stream_1; +extern MSerialT serial_stream_2; +extern MSerialT serial_stream_3; + +#define _MSERIAL(X) serial_stream_##X +#define MSERIAL(X) _MSERIAL(X) + +#if WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "SERIAL_PORT must be from 0 to 3. Please update your configuration." +#endif + +#ifdef SERIAL_PORT_2 + #if WITHIN(SERIAL_PORT_2, 0, 3) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 3. Please update your configuration." + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if WITHIN(MMU2_SERIAL_PORT, 0, 3) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #error "MMU2_SERIAL_PORT must be from 0 to 3. Please update your configuration." + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if WITHIN(LCD_SERIAL_PORT, 0, 3) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #error "LCD_SERIAL_PORT must be from 0 to 3. Please update your configuration." + #endif +#endif + +// ------------------------ +// Interrupts +// ------------------------ + +#define CRITICAL_SECTION_START() +#define CRITICAL_SECTION_END() + +// ------------------------ +// ADC +// ------------------------ + +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 + +/* ---------------- Delay in cycles */ + +#define DELAY_CYCLES(x) Kernel::delayCycles(x) +#define SYSTEM_YIELD() Kernel::yield() + +// Maple Compatibility +typedef void (*systickCallback_t)(void); +void systick_attach_callback(systickCallback_t cb); +extern volatile uint32_t systick_uptime_millis; + +// Marlin uses strstr in constexpr context, this is not supported, workaround by defining constexpr versions of the required functions. +#define strstr(a, b) strstr_constexpr((a), (b)) + +constexpr inline std::size_t strlen_constexpr(const char* str) { + // https://github.com/gcc-mirror/gcc/blob/5c7634a0e5f202935aa6c11b6ea953b8bf80a00a/libstdc%2B%2B-v3/include/bits/char_traits.h#L329 + if (str != nullptr) { + std::size_t i = 0; + while (str[i] != '\0') ++i; + return i; + } + return 0; +} + +constexpr inline int strncmp_constexpr(const char* lhs, const char* rhs, std::size_t count) { + // https://github.com/gcc-mirror/gcc/blob/13b9cbfc32fe3ac4c81c4dd9c42d141c8fb95db4/libstdc%2B%2B-v3/include/bits/char_traits.h#L655 + if (lhs == nullptr || rhs == nullptr) + return rhs != nullptr ? -1 : 1; + + for (std::size_t i = 0; i < count; ++i) + if (lhs[i] != rhs[i]) + return lhs[i] < rhs[i] ? -1 : 1; + else if (lhs[i] == '\0') + return 0; + + return 0; +} + +constexpr inline const char* strstr_constexpr(const char* str, const char* target) { + // https://github.com/freebsd/freebsd/blob/master/sys/libkern/strstr.c + if (char c = target != nullptr ? *target++ : '\0'; c != '\0' && str != nullptr) { + std::size_t len = strlen_constexpr(target); + do { + char sc = {}; + do { + if ((sc = *str++) == '\0') return nullptr; + } while (sc != c); + } while (strncmp_constexpr(str, target, len) != 0); + --str; + } + return str; +} + +constexpr inline char* strstr_constexpr(char* str, const char* target) { + // https://github.com/freebsd/freebsd/blob/master/sys/libkern/strstr.c + if (char c = target != nullptr ? *target++ : '\0'; c != '\0' && str != nullptr) { + std::size_t len = strlen_constexpr(target); + do { + char sc = {}; + do { + if ((sc = *str++) == '\0') return nullptr; + } while (sc != c); + } while (strncmp_constexpr(str, target, len) != 0); + --str; + } + return str; +} + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return true; } + static void isr_on() {} + static void isr_off() {} + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static constexpr uint8_t reset_reason = RST_POWER_ON; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint8_t active_ch; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch); + + // Begin ADC sampling on the given channel. Called from Temperature::isr! + static void adc_start(const uint8_t ch); + + // Is the ADC ready for reading? + static bool adc_ready(); + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/src/HAL/NATIVE_SIM/MarlinSPI.h b/src/HAL/NATIVE_SIM/MarlinSPI.h new file mode 100644 index 0000000..b5cc6f0 --- /dev/null +++ b/src/HAL/NATIVE_SIM/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/NATIVE_SIM/fastio.h b/src/HAL/NATIVE_SIM/fastio.h new file mode 100644 index 0000000..de8013b --- /dev/null +++ b/src/HAL/NATIVE_SIM/fastio.h @@ -0,0 +1,111 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for X86_64 + */ + +#include "../shared/Marduino.h" +#include + +#define SET_DIR_INPUT(IO) Gpio::setDir(IO, 1) +#define SET_DIR_OUTPUT(IO) Gpio::setDir(IO, 0) + +#define SET_MODE(IO, mode) Gpio::setMode(IO, mode) + +#define WRITE_PIN_SET(IO) Gpio::set(IO) +#define WRITE_PIN_CLR(IO) Gpio::clear(IO) + +#define READ_PIN(IO) Gpio::get(IO) +#define WRITE_PIN(IO,V) Gpio::set(IO, V) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(STEP); WRITE(STEP, HIGH); WRITE(STEP, LOW); + * + * Why double up on these macros? see http://gcc.gnu.org/onlinedocs/cpp/Stringification.html + */ + +/// Read a pin +#define _READ(IO) READ_PIN(IO) + +/// Write to a pin +#define _WRITE(IO,V) WRITE_PIN(IO,V) + +/// toggle a pin +#define _TOGGLE(IO) _WRITE(IO, !READ(IO)) + +/// set pin as input +#define _SET_INPUT(IO) SET_DIR_INPUT(IO) + +/// set pin as output +#define _SET_OUTPUT(IO) SET_DIR_OUTPUT(IO) + +/// set pin as input with pullup mode +#define _PULLUP(IO,V) pinMode(IO, (V) ? INPUT_PULLUP : INPUT) + +/// set pin as input with pulldown mode +#define _PULLDOWN(IO,V) pinMode(IO, (V) ? INPUT_PULLDOWN : INPUT) + +// hg42: all pins can be input or output (I hope) +// hg42: undefined pins create compile error (IO, is no pin) +// hg42: currently not used, but was used by pinsDebug + +/// check if pin is an input +#define _IS_INPUT(IO) (IO >= 0) + +/// check if pin is an output +#define _IS_OUTPUT(IO) (IO >= 0) + +/// Read a pin wrapper +#define READ(IO) _READ(IO) + +/// Write to a pin wrapper +#define WRITE(IO,V) _WRITE(IO,V) + +/// toggle a pin wrapper +#define TOGGLE(IO) _TOGGLE(IO) + +/// set pin as input wrapper +#define SET_INPUT(IO) _SET_INPUT(IO) +/// set pin as input with pullup wrapper +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) +/// set pin as input with pulldown wrapper +#define SET_INPUT_PULLDOWN(IO) do{ _SET_INPUT(IO); _PULLDOWN(IO, HIGH); }while(0) +/// set pin as output wrapper - reads the pin and sets the output to that value +#define SET_OUTPUT(IO) do{ _WRITE(IO, _READ(IO)); _SET_OUTPUT(IO); }while(0) +// set pin as PWM +#define SET_PWM(IO) SET_OUTPUT(IO) + +/// check if pin is an input wrapper +#define IS_INPUT(IO) _IS_INPUT(IO) +/// check if pin is an output wrapper +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h b/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h new file mode 100644 index 0000000..1ac02f1 --- /dev/null +++ b/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h b/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h new file mode 100644 index 0000000..69b6b48 --- /dev/null +++ b/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// Add strcmp_P if missing +#ifndef strcmp_P + #define strcmp_P(a, b) strcmp((a), (b)) +#endif + +#ifndef strcat_P + #define strcat_P(dest, src) strcat((dest), (src)) +#endif diff --git a/src/HAL/NATIVE_SIM/inc/Conditionals_post.h b/src/HAL/NATIVE_SIM/inc/Conditionals_post.h new file mode 100644 index 0000000..1ac02f1 --- /dev/null +++ b/src/HAL/NATIVE_SIM/inc/Conditionals_post.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/NATIVE_SIM/inc/SanityCheck.h b/src/HAL/NATIVE_SIM/inc/SanityCheck.h new file mode 100644 index 0000000..2d7bef2 --- /dev/null +++ b/src/HAL/NATIVE_SIM/inc/SanityCheck.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test X86_64-specific configuration values for errors at compile-time. + */ + +// Emulating RAMPS +#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) + #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on LINUX." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on LINUX." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on LINUX." +#endif diff --git a/src/HAL/NATIVE_SIM/pinsDebug.h b/src/HAL/NATIVE_SIM/pinsDebug.h new file mode 100644 index 0000000..aa90eb3 --- /dev/null +++ b/src/HAL/NATIVE_SIM/pinsDebug.h @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Support routines for X86_64 + */ +#pragma once + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS +#define pwm_details(pin) pin = pin // do nothing // print PWM details +#define pwm_status(pin) false //Print a pin's PWM status. Return true if it's currently a PWM pin. +#define IS_ANALOG(P) (DIGITAL_PIN_TO_ANALOG_PIN(P) >= 0 ? 1 : 0) +#define digitalRead_mod(p) digitalRead(p) +#define PRINT_PORT(p) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin + +// active ADC function/mode/code values for PINSEL registers +inline constexpr int8_t ADC_pin_mode(pin_t pin) { + return (-1); +} + +inline int8_t get_pin_mode(pin_t pin) { + if (!VALID_PIN(pin)) return -1; + return 0; +} + +inline bool GET_PINMODE(pin_t pin) { + int8_t pin_mode = get_pin_mode(pin); + if (pin_mode == -1 || pin_mode == ADC_pin_mode(pin)) // found an invalid pin or active analog pin + return false; + + return (Gpio::getMode(pin) != 0); //input/output state +} + +inline bool GET_ARRAY_IS_DIGITAL(pin_t pin) { + return (!IS_ANALOG(pin) || get_pin_mode(pin) != ADC_pin_mode(pin)); +} diff --git a/src/HAL/NATIVE_SIM/servo_private.h b/src/HAL/NATIVE_SIM/servo_private.h new file mode 100644 index 0000000..06be189 --- /dev/null +++ b/src/HAL/NATIVE_SIM/servo_private.h @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * Based on "servo.h - Interrupt driven Servo library for Arduino using 16 bit timers - + * Version 2 Copyright (c) 2009 Michael Margolis. All right reserved. + * + * The only modification was to update/delete macros to match the LPC176x. + * + */ + +#include + +// Macros +//values in microseconds +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds + +#define MAX_SERVOS 4 + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + + +// Types + +typedef struct { + uint8_t nbr : 8 ; // a pin number from 0 to 254 (255 signals invalid pin) + uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t; + +typedef struct { + ServoPin_t Pin; + unsigned int pulse_width; // pulse width in microseconds +} ServoInfo_t; + +// Global variables + +extern uint8_t ServoCount; +extern ServoInfo_t servo_info[MAX_SERVOS]; diff --git a/src/HAL/NATIVE_SIM/spi_pins.h b/src/HAL/NATIVE_SIM/spi_pins.h new file mode 100644 index 0000000..a5138e0 --- /dev/null +++ b/src/HAL/NATIVE_SIM/spi_pins.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/macros.h" +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(HAS_MARLINUI_U8GLIB, SDSUPPORT) && (LCD_PINS_D4 == SD_SCK_PIN || LCD_PINS_ENABLE == SD_MOSI_PIN || DOGLCD_SCK == SD_SCK_PIN || DOGLCD_MOSI == SD_MOSI_PIN) + #define LPC_SOFTWARE_SPI // If the SD card and LCD adapter share the same SPI pins, then software SPI is currently + // needed due to the speed and mode required for communicating with each device being different. + // This requirement can be removed if the SPI access to these devices is updated to use + // spiBeginTransaction. +#endif + +// Onboard SD +//#define SD_SCK_PIN P0_07 +//#define SD_MISO_PIN P0_08 +//#define SD_MOSI_PIN P0_09 +//#define SD_SS_PIN P0_06 + +// External SD +#ifndef SD_SCK_PIN + #define SD_SCK_PIN 50 +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN 51 +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN 52 +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN 53 +#endif +#ifndef SDSS + #define SDSS SD_SS_PIN +#endif diff --git a/src/HAL/NATIVE_SIM/tft/tft_spi.h b/src/HAL/NATIVE_SIM/tft/tft_spi.h new file mode 100644 index 0000000..b3e622f --- /dev/null +++ b/src/HAL/NATIVE_SIM/tft/tft_spi.h @@ -0,0 +1,64 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT 8 +#define DATASIZE_16BIT 16 +#define TFT_IO_DRIVER TFT_SPI + +#define DMA_MINC_ENABLE 1 +#define DMA_MINC_DISABLE 0 + +class TFT_SPI { +private: + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + +public: + // static SPIClass SPIx; + + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATASIZE_16BIT); + static void DataTransferEnd(); + static void DataTransferAbort(); + + static void WriteData(uint16_t Data); + static void WriteReg(uint16_t Reg); + + static void WriteSequence(uint16_t *Data, uint16_t Count); + // static void WriteMultiple(uint16_t Color, uint16_t Count); + static void WriteMultiple(uint16_t Color, uint32_t Count); +}; diff --git a/src/HAL/NATIVE_SIM/tft/xpt2046.h b/src/HAL/NATIVE_SIM/tft/xpt2046.h new file mode 100644 index 0000000..b131853 --- /dev/null +++ b/src/HAL/NATIVE_SIM/tft/xpt2046.h @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include +#endif + +#ifndef TOUCH_MISO_PIN + #define TOUCH_MISO_PIN SD_MISO_PIN +#endif +#ifndef TOUCH_MOSI_PIN + #define TOUCH_MOSI_PIN SD_MOSI_PIN +#endif +#ifndef TOUCH_SCK_PIN + #define TOUCH_SCK_PIN SD_SCK_PIN +#endif +#ifndef TOUCH_CS_PIN + #define TOUCH_CS_PIN SD_SS_PIN +#endif +#ifndef TOUCH_INT_PIN + #define TOUCH_INT_PIN -1 +#endif + +#define XPT2046_DFR_MODE 0x00 +#define XPT2046_SER_MODE 0x04 +#define XPT2046_CONTROL 0x80 + +enum XPTCoordinate : uint8_t { + XPT2046_X = 0x10 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Y = 0x50 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z1 = 0x30 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, +}; + +#if !defined(XPT2046_Z1_THRESHOLD) + #define XPT2046_Z1_THRESHOLD 10 +#endif + +class XPT2046 { +private: + static bool isBusy() { return false; } + + static uint16_t getRawData(const XPTCoordinate coordinate); + static bool isTouched(); + + static void DataTransferBegin(); + static void DataTransferEnd(); + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static uint16_t HardwareIO(uint16_t data); + #endif + static uint16_t SoftwareIO(uint16_t data); + static uint16_t IO(uint16_t data = 0); + +public: + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static SPIClass SPIx; + #endif + + static void Init(); + static bool getRawPoint(int16_t *x, int16_t *y); +}; diff --git a/src/HAL/NATIVE_SIM/timers.h b/src/HAL/NATIVE_SIM/timers.h new file mode 100644 index 0000000..be38d58 --- /dev/null +++ b/src/HAL/NATIVE_SIM/timers.h @@ -0,0 +1,91 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL timers for Linux X86_64 + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint64_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFFFFFFFFFF + +#define HAL_TIMER_RATE ((SystemCoreClock) / 4) // frequency of timers peripherals + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif +#ifndef MF_TIMER_SYSTICK + #define MF_TIMER_SYSTICK 2 // Timer Index for Systick +#endif +#define SYSTICK_TIMER_FREQUENCY 1000 + +#define TEMP_TIMER_RATE 1000000 +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void TIMER0_IRQHandler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void TIMER1_IRQHandler() +#endif + +void HAL_timer_init(); +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare); +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num); +hal_timer_t HAL_timer_get_count(const uint8_t timer_num); + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp b/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp new file mode 100644 index 0000000..7454543 --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp @@ -0,0 +1,52 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +// adapted from I2C/master/master.c example +// https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html + +#ifdef __PLAT_NATIVE_SIM__ + +#include + +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t u8g_i2c_start(const uint8_t sla) { + return 1; +} + +void u8g_i2c_init(const uint8_t clock_option) { +} + +uint8_t u8g_i2c_send_byte(uint8_t data) { + return 1; +} + +void u8g_i2c_stop() { +} + +#ifdef __cplusplus + } +#endif + +#endif // __PLAT_NATIVE_SIM__ diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h b/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h new file mode 100644 index 0000000..6d5f91d --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifdef __cplusplus + extern "C" { +#endif + +void u8g_i2c_init(const uint8_t clock_options); +//uint8_t u8g_i2c_wait(uint8_t mask, uint8_t pos); +uint8_t u8g_i2c_start(uint8_t sla); +uint8_t u8g_i2c_send_byte(uint8_t data); +void u8g_i2c_stop(); + +#ifdef __cplusplus + } +#endif + diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_defines.h b/src/HAL/NATIVE_SIM/u8g/LCD_defines.h new file mode 100644 index 0000000..44ffbfe --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_defines.h @@ -0,0 +1,44 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void usleep(uint64_t microsec); +// The following are optional depending on the platform. + +// definitions of HAL specific com and device drivers. +uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); + +// connect U8g com generic com names to the desired driver +#define U8G_COM_SW_SPI u8g_com_sw_spi_fn +#define U8G_COM_ST7920_SW_SPI u8g_com_ST7920_sw_spi_fn + +// let these default for now +#define U8G_COM_HW_SPI u8g_com_null_fn +#define U8G_COM_ST7920_HW_SPI u8g_com_null_fn +#define U8G_COM_SSD_I2C u8g_com_null_fn +#define U8G_COM_PARALLEL u8g_com_null_fn +#define U8G_COM_T6963 u8g_com_null_fn +#define U8G_COM_FAST_PARALLEL u8g_com_null_fn +#define U8G_COM_UC_I2C u8g_com_null_fn + + diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_delay.h b/src/HAL/NATIVE_SIM/u8g/LCD_delay.h new file mode 100644 index 0000000..297361c --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_delay.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * LCD delay routines - used by all the drivers. + * + * These are based on the LPC1768 routines. + * + * Couldn't just call exact copies because the overhead + * results in a one microsecond delay taking about 4µS. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +void U8g_delay(int msec); +void u8g_MicroDelay(); +void u8g_10MicroDelay(); + +#ifdef __cplusplus + } +#endif diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp b/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp new file mode 100644 index 0000000..91b7e0f --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp @@ -0,0 +1,52 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../fastio.h" +#include "LCD_pin_routines.h" + +#ifdef __cplusplus + extern "C" { +#endif + +void u8g_SetPinOutput(uint8_t internal_pin_number) { SET_DIR_OUTPUT(internal_pin_number); } +void u8g_SetPinInput(uint8_t internal_pin_number) { SET_DIR_INPUT(internal_pin_number); } +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status) { WRITE_PIN(pin, pin_status); } +uint8_t u8g_GetPinLevel(uint8_t pin) { return READ_PIN(pin); } +void usleep(uint64_t microsec) { assert(false); /* why we here? */ } + +#ifdef __cplusplus + } +#endif + +#endif // __PLAT_NATIVE_SIM__ diff --git a/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h b/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h new file mode 100644 index 0000000..c27c84e --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h @@ -0,0 +1,46 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + + +#ifdef __cplusplus + extern "C" { +#endif + +void u8g_SetPinOutput(uint8_t internal_pin_number); +void u8g_SetPinInput(uint8_t internal_pin_number); +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status); +uint8_t u8g_GetPinLevel(uint8_t pin); + +#ifdef __cplusplus + } +#endif diff --git a/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp b/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp new file mode 100644 index 0000000..c384cdd --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp @@ -0,0 +1,171 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../../../inc/MarlinConfig.h" + +#if IS_U8GLIB_ST7920 + +#include +#include "../../shared/Delay.h" + +#undef SPI_SPEED +#define SPI_SPEED 6 +#define SPI_DELAY_CYCLES (1 + SPI_SPEED * 10) + +static pin_t SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL; +static uint8_t SPI_speed = 0; + +static uint8_t swSpiTransfer(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin) { + for (uint8_t i = 0; i < 8; i++) { + WRITE_PIN(mosi_pin, !!(b & 0x80)); + DELAY_CYCLES(SPI_SPEED); + WRITE_PIN(sck_pin, HIGH); + DELAY_CYCLES(SPI_SPEED); + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + WRITE_PIN(sck_pin, LOW); + DELAY_CYCLES(SPI_SPEED); + } + return b; +} + +static uint8_t swSpiInit(const uint8_t spiRate, const pin_t sck_pin, const pin_t mosi_pin) { + WRITE_PIN(mosi_pin, HIGH); + WRITE_PIN(sck_pin, LOW); + return spiRate; +} + +static void u8g_com_st7920_write_byte_sw_spi(uint8_t rs, uint8_t val) { + static uint8_t rs_last_state = 255; + if (rs != rs_last_state) { + // Transfer Data (FA) or Command (F8) + swSpiTransfer(rs ? 0x0FA : 0x0F8, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + rs_last_state = rs; + DELAY_US(40); // Give the controller time to process the data: 20 is bad, 30 is OK, 40 is safe + } + swSpiTransfer(val & 0x0F0, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + swSpiTransfer(val << 4, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); +} +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t u8g_com_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SCK_pin_ST7920_HAL = u8g->pin_list[U8G_PI_SCK]; + MOSI_pin_ST7920_HAL_HAL = u8g->pin_list[U8G_PI_MOSI]; + + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_Delay(5); + + SPI_speed = swSpiInit(SPI_SPEED, SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL); + + u8g_SetPILevel(u8g, U8G_PI_CS, 0); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + + u8g->pin_list[U8G_PI_A0_STATE] = 0; /* initial RS state: command mode */ + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + break; + + case U8G_COM_MSG_CHIP_SELECT: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_CS]) u8g_SetPILevel(u8g, U8G_PI_CS, arg_val); //note: the st7920 has an active high chip select + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + } + return 1; +} +#ifdef __cplusplus + } +#endif + +#endif // IS_U8GLIB_ST7920 +#endif // TARGET_LPC1768 diff --git a/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp b/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp new file mode 100644 index 0000000..7be8458 --- /dev/null +++ b/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp @@ -0,0 +1,218 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Based on u8g_com_std_sw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2015, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#undef SPI_SPEED +#define SPI_SPEED 2 // About 2 MHz + +#include +#include + +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t swSpiTransfer_mode_0(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + LOOP_L_N(i, 8) { + if (spi_speed == 0) { + WRITE_PIN(mosi_pin, !!(b & 0x80)); + WRITE_PIN(sck_pin, HIGH); + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + WRITE_PIN(sck_pin, LOW); + } + else { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + LOOP_L_N(j, spi_speed) + WRITE_PIN(mosi_pin, state); + + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE_PIN(sck_pin, HIGH); + + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + + LOOP_L_N(j, spi_speed) + WRITE_PIN(sck_pin, LOW); + } + } + + return b; +} + +uint8_t swSpiTransfer_mode_3(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + + LOOP_L_N(i, 8) { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + if (spi_speed == 0) { + WRITE_PIN(sck_pin, LOW); + WRITE_PIN(mosi_pin, state); + WRITE_PIN(mosi_pin, state); // need some setup time + WRITE_PIN(sck_pin, HIGH); + } + else { + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE_PIN(sck_pin, LOW); + + LOOP_L_N(j, spi_speed) + WRITE_PIN(mosi_pin, state); + + LOOP_L_N(j, spi_speed) + WRITE_PIN(sck_pin, HIGH); + } + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + } + + return b; +} + +static uint8_t SPI_speed = 0; + +static uint8_t swSpiInit(const uint8_t spi_speed, const uint8_t clk_pin, const uint8_t mosi_pin) { + return spi_speed; +} + +static void u8g_sw_spi_shift_out(uint8_t dataPin, uint8_t clockPin, uint8_t val) { + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) + swSpiTransfer_mode_3(val, SPI_speed, clockPin, -1, dataPin); + #else + swSpiTransfer_mode_0(val, SPI_speed, clockPin, -1, dataPin); + #endif +} + +uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_A0); + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPIOutput(u8g, U8G_PI_RESET); + SPI_speed = swSpiInit(SPI_SPEED, u8g->pin_list[U8G_PI_SCK], u8g->pin_list[U8G_PI_MOSI]); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) // LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + u8g_SetPILevel(u8g, U8G_PI_SCK, 1); // Set SCK to mode 3 idle state before CS goes active + u8g_SetPILevel(u8g, U8G_PI_CS, LOW); + } + else { + u8g_SetPILevel(u8g, U8G_PI_CS, HIGH); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); // Set SCK to mode 0 idle state after CS goes inactive + } + #else + u8g_SetPILevel(u8g, U8G_PI_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel(u8g, U8G_PI_A0, arg_val); + break; + } + return 1; +} + +#ifdef __cplusplus + } +#endif + +#elif NONE(TFT_COLOR_UI, TFT_CLASSIC_UI, TFT_LVGL_UI, HAS_MARLINUI_HD44780) && HAS_MARLINUI_U8GLIB + + #include + uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) {return 0;} + +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#endif // __PLAT_NATIVE_SIM__ diff --git a/src/HAL/SAMD51/HAL.cpp b/src/HAL/SAMD51/HAL.cpp new file mode 100644 index 0000000..bd1c98b --- /dev/null +++ b/src/HAL/SAMD51/HAL.cpp @@ -0,0 +1,690 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __SAMD51__ + +#include "../../inc/MarlinConfig.h" +#include +#include + +#ifdef ADAFRUIT_GRAND_CENTRAL_M4 + #if USING_HW_SERIALUSB + DefaultSerial1 MSerial0(false, Serial); + #endif + #if USING_HW_SERIAL0 + DefaultSerial2 MSerial1(false, Serial1); + #endif + #if USING_HW_SERIAL1 + DefaultSerial3 MSerial2(false, Serial2); + #endif + #if USING_HW_SERIAL2 + DefaultSerial4 MSerial3(false, Serial3); + #endif + #if USING_HW_SERIAL3 + DefaultSerial5 MSerial4(false, Serial4); + #endif +#endif + +#define GET_TEMP_0_ADC() TERN(HAS_TEMP_ADC_0, PIN_TO_ADC(TEMP_0_PIN), -1) +#define GET_TEMP_1_ADC() TERN(HAS_TEMP_ADC_1, PIN_TO_ADC(TEMP_1_PIN), -1) +#define GET_TEMP_2_ADC() TERN(HAS_TEMP_ADC_2, PIN_TO_ADC(TEMP_2_PIN), -1) +#define GET_TEMP_3_ADC() TERN(HAS_TEMP_ADC_3, PIN_TO_ADC(TEMP_3_PIN), -1) +#define GET_TEMP_4_ADC() TERN(HAS_TEMP_ADC_4, PIN_TO_ADC(TEMP_4_PIN), -1) +#define GET_TEMP_5_ADC() TERN(HAS_TEMP_ADC_5, PIN_TO_ADC(TEMP_5_PIN), -1) +#define GET_TEMP_6_ADC() TERN(HAS_TEMP_ADC_6, PIN_TO_ADC(TEMP_6_PIN), -1) +#define GET_TEMP_7_ADC() TERN(HAS_TEMP_ADC_7, PIN_TO_ADC(TEMP_7_PIN), -1) +#define GET_BED_ADC() TERN(HAS_TEMP_ADC_BED, PIN_TO_ADC(TEMP_BED_PIN), -1) +#define GET_CHAMBER_ADC() TERN(HAS_TEMP_ADC_CHAMBER, PIN_TO_ADC(TEMP_CHAMBER_PIN), -1) +#define GET_PROBE_ADC() TERN(HAS_TEMP_ADC_PROBE, PIN_TO_ADC(TEMP_PROBE_PIN), -1) +#define GET_COOLER_ADC() TERN(HAS_TEMP_ADC_COOLER, PIN_TO_ADC(TEMP_COOLER_PIN), -1) +#define GET_BOARD_ADC() TERN(HAS_TEMP_ADC_BOARD, PIN_TO_ADC(TEMP_BOARD_PIN), -1) +#define GET_FILAMENT_WIDTH_ADC() TERN(FILAMENT_WIDTH_SENSOR, PIN_TO_ADC(FILWIDTH_PIN), -1) +#define GET_BUTTONS_ADC() TERN(HAS_ADC_BUTTONS, PIN_TO_ADC(ADC_KEYPAD_PIN), -1) +#define GET_JOY_ADC_X() TERN(HAS_JOY_ADC_X, PIN_TO_ADC(JOY_X_PIN), -1) +#define GET_JOY_ADC_Y() TERN(HAS_JOY_ADC_Y, PIN_TO_ADC(JOY_Y_PIN), -1) +#define GET_JOY_ADC_Z() TERN(HAS_JOY_ADC_Z, PIN_TO_ADC(JOY_Z_PIN), -1) + +#define IS_ADC_REQUIRED(n) ( \ + GET_TEMP_0_ADC() == n || GET_TEMP_1_ADC() == n || GET_TEMP_2_ADC() == n || GET_TEMP_3_ADC() == n \ + || GET_TEMP_4_ADC() == n || GET_TEMP_5_ADC() == n || GET_TEMP_6_ADC() == n || GET_TEMP_7_ADC() == n \ + || GET_BED_ADC() == n \ + || GET_CHAMBER_ADC() == n \ + || GET_PROBE_ADC() == n \ + || GET_COOLER_ADC() == n \ + || GET_BOARD_ADC() == n \ + || GET_FILAMENT_WIDTH_ADC() == n \ + || GET_BUTTONS_ADC() == n \ + || GET_JOY_ADC_X() == n || GET_JOY_ADC_Y() == n || GET_JOY_ADC_Z() == n \ +) + +#if IS_ADC_REQUIRED(0) + #define ADC0_IS_REQUIRED 1 + #define FIRST_ADC 0 +#else + #define FIRST_ADC 1 +#endif +#if IS_ADC_REQUIRED(1) + #define ADC1_IS_REQUIRED 1 + #define LAST_ADC 1 +#else + #define LAST_ADC 0 +#endif +#if ADC0_IS_REQUIRED || ADC1_IS_REQUIRED + #define ADC_IS_REQUIRED 1 + #define DMA_IS_REQUIRED 1 +#endif + +enum ADCIndex { + #if GET_TEMP_0_ADC() == 0 + TEMP_0, + #endif + #if GET_TEMP_1_ADC() == 0 + TEMP_1, + #endif + #if GET_TEMP_2_ADC() == 0 + TEMP_2, + #endif + #if GET_TEMP_3_ADC() == 0 + TEMP_3, + #endif + #if GET_TEMP_4_ADC() == 0 + TEMP_4, + #endif + #if GET_TEMP_5_ADC() == 0 + TEMP_5, + #endif + #if GET_TEMP_6_ADC() == 0 + TEMP_6, + #endif + #if GET_TEMP_7_ADC() == 0 + TEMP_7, + #endif + #if GET_BED_ADC() == 0 + TEMP_BED, + #endif + #if GET_CHAMBER_ADC() == 0 + TEMP_CHAMBER, + #endif + #if GET_PROBE_ADC() == 0 + TEMP_PROBE, + #endif + #if GET_COOLER_ADC() == 0 + TEMP_COOLER, + #endif + #if GET_BOARD_ADC() == 0 + TEMP_BOARD, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 0 + FILWIDTH, + #endif + #if GET_BUTTONS_ADC() == 0 + ADC_KEY, + #endif + #if GET_JOY_ADC_X() == 0 + JOY_X, + #endif + #if GET_JOY_ADC_Y() == 0 + JOY_Y, + #endif + #if GET_JOY_ADC_Z() == 0 + JOY_Z, + #endif + #if GET_TEMP_0_ADC() == 1 + TEMP_0, + #endif + #if GET_TEMP_1_ADC() == 1 + TEMP_1, + #endif + #if GET_TEMP_2_ADC() == 1 + TEMP_2, + #endif + #if GET_TEMP_3_ADC() == 1 + TEMP_3, + #endif + #if GET_TEMP_4_ADC() == 1 + TEMP_4, + #endif + #if GET_TEMP_5_ADC() == 1 + TEMP_5, + #endif + #if GET_TEMP_6_ADC() == 1 + TEMP_6, + #endif + #if GET_TEMP_7_ADC() == 1 + TEMP_7, + #endif + #if GET_BED_ADC() == 1 + TEMP_BED, + #endif + #if GET_CHAMBER_ADC() == 1 + TEMP_CHAMBER, + #endif + #if GET_PROBE_ADC() == 1 + TEMP_PROBE, + #endif + #if GET_COOLER_ADC() == 1 + TEMP_COOLER, + #endif + #if GET_BOARD_ADC() == 1 + TEMP_BOARD, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 1 + FILWIDTH, + #endif + #if GET_BUTTONS_ADC() == 1 + ADC_KEY, + #endif + #if GET_JOY_ADC_X() == 1 + JOY_X, + #endif + #if GET_JOY_ADC_Y() == 1 + JOY_Y, + #endif + #if GET_JOY_ADC_Z() == 1 + JOY_Z, + #endif + ADC_COUNT +}; + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT_REG TERN(WATCHDOG_DURATION_8S, WDT_CONFIG_PER_CYC8192, WDT_CONFIG_PER_CYC4096) // 4 or 8 second timeout + + void MarlinHAL::watchdog_init() { + // The low-power oscillator used by the WDT runs at 32,768 Hz with + // a 1:32 prescale, thus 1024 Hz, though probably not super precise. + + // Setup WDT clocks + MCLK->APBAMASK.bit.OSC32KCTRL_ = true; + MCLK->APBAMASK.bit.WDT_ = true; + OSC32KCTRL->OSCULP32K.bit.EN1K = true; // Enable out 1K (this is what WDT uses) + + WDT->CTRLA.bit.ENABLE = false; // Disable watchdog for config + SYNC(WDT->SYNCBUSY.bit.ENABLE); + + WDT->INTENCLR.reg = WDT_INTENCLR_EW; // Disable early warning interrupt + WDT->CONFIG.reg = WDT_TIMEOUT_REG; // Set a 4s or 8s period for chip reset + + hal.watchdog_refresh(); + + WDT->CTRLA.reg = WDT_CTRLA_ENABLE; // Start watchdog now in normal mode + SYNC(WDT->SYNCBUSY.bit.ENABLE); + } + + // Reset watchdog. MUST be called at least every 4 seconds after the + // first watchdog_init or SAMD will go into emergency procedures. + void MarlinHAL::watchdog_refresh() { + SYNC(WDT->SYNCBUSY.bit.CLEAR); // Test first if previous is 'ongoing' to save time waiting for command execution + WDT->CLEAR.reg = WDT_CLEAR_CLEAR_KEY; + } + +#endif + +// ------------------------ +// Types +// ------------------------ + +#if DMA_IS_REQUIRED + + // Struct must be 32 bits aligned because of DMA accesses but fields needs to be 8 bits packed + typedef struct __attribute__((aligned(4), packed)) { + ADC_INPUTCTRL_Type INPUTCTRL; + } HAL_DMA_DAC_Registers; // DMA transferred registers + +#endif + +// ------------------------ +// Private Variables +// ------------------------ + +#if ADC_IS_REQUIRED + + // Pins used by ADC inputs. Order must be ADC0 inputs first then ADC1 + static constexpr uint8_t adc_pins[ADC_COUNT] = { + // ADC0 pins + #if GET_TEMP_0_ADC() == 0 + TEMP_0_PIN, + #endif + #if GET_TEMP_1_ADC() == 0 + TEMP_1_PIN, + #endif + #if GET_TEMP_2_ADC() == 0 + TEMP_2_PIN, + #endif + #if GET_TEMP_3_ADC() == 0 + TEMP_3_PIN, + #endif + #if GET_TEMP_4_ADC() == 0 + TEMP_4_PIN, + #endif + #if GET_TEMP_5_ADC() == 0 + TEMP_5_PIN, + #endif + #if GET_TEMP_6_ADC() == 0 + TEMP_6_PIN, + #endif + #if GET_TEMP_7_ADC() == 0 + TEMP_7_PIN, + #endif + #if GET_BED_ADC() == 0 + TEMP_BED_PIN, + #endif + #if GET_CHAMBER_ADC() == 0 + TEMP_CHAMBER_PIN, + #endif + #if GET_PROBE_ADC() == 0 + TEMP_PROBE_PIN, + #endif + #if GET_COOLER_ADC() == 0 + TEMP_COOLER_PIN, + #endif + #if GET_BOARD_ADC() == 0 + TEMP_BOARD_PIN, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 0 + FILWIDTH_PIN, + #endif + #if GET_BUTTONS_ADC() == 0 + ADC_KEYPAD_PIN, + #endif + #if GET_JOY_ADC_X() == 0 + JOY_X_PIN, + #endif + #if GET_JOY_ADC_Y() == 0 + JOY_Y_PIN, + #endif + #if GET_JOY_ADC_Z() == 0 + JOY_Z_PIN, + #endif + // ADC1 pins + #if GET_TEMP_0_ADC() == 1 + TEMP_0_PIN, + #endif + #if GET_TEMP_1_ADC() == 1 + TEMP_1_PIN, + #endif + #if GET_TEMP_2_ADC() == 1 + TEMP_2_PIN, + #endif + #if GET_TEMP_3_ADC() == 1 + TEMP_3_PIN, + #endif + #if GET_TEMP_4_ADC() == 1 + TEMP_4_PIN, + #endif + #if GET_TEMP_5_ADC() == 1 + TEMP_5_PIN, + #endif + #if GET_TEMP_6_ADC() == 1 + TEMP_6_PIN, + #endif + #if GET_TEMP_7_ADC() == 1 + TEMP_7_PIN, + #endif + #if GET_BED_ADC() == 1 + TEMP_BED_PIN, + #endif + #if GET_CHAMBER_ADC() == 1 + TEMP_CHAMBER_PIN, + #endif + #if GET_PROBE_ADC() == 1 + TEMP_PROBE_PIN, + #endif + #if GET_COOLER_ADC() == 1 + TEMP_COOLER_PIN, + #endif + #if GET_BOARD_ADC() == 1 + TEMP_BOARD_PIN, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 1 + FILWIDTH_PIN, + #endif + #if GET_BUTTONS_ADC() == 1 + ADC_KEYPAD_PIN, + #endif + #if GET_JOY_ADC_X() == 1 + JOY_X_PIN, + #endif + #if GET_JOY_ADC_Y() == 1 + JOY_Y_PIN, + #endif + #if GET_JOY_ADC_Z() == 1 + JOY_Z_PIN, + #endif + }; + + static uint16_t adc_results[ADC_COUNT]; + + #if ADC0_IS_REQUIRED + Adafruit_ZeroDMA adc0DMAProgram, adc0DMARead; + + static constexpr HAL_DMA_DAC_Registers adc0_dma_regs_list[ADC_COUNT] = { + #if GET_TEMP_0_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_0_PIN) }, + #endif + #if GET_TEMP_1_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_1_PIN) }, + #endif + #if GET_TEMP_2_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_2_PIN) }, + #endif + #if GET_TEMP_3_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_3_PIN) }, + #endif + #if GET_TEMP_4_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_4_PIN) }, + #endif + #if GET_TEMP_5_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_5_PIN) }, + #endif + #if GET_TEMP_6_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_6_PIN) }, + #endif + #if GET_TEMP_7_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_7_PIN) }, + #endif + #if GET_BED_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_BED_PIN) }, + #endif + #if GET_CHAMBER_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_CHAMBER_PIN) }, + #endif + #if GET_PROBE_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, + #endif + #if GET_COOLER_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_COOLER_PIN) }, + #endif + #if GET_BOARD_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_BOARD_PIN) }, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 0 + { PIN_TO_INPUTCTRL(FILWIDTH_PIN) }, + #endif + #if GET_BUTTONS_ADC() == 0 + { PIN_TO_INPUTCTRL(ADC_KEYPAD_PIN) }, + #endif + #if GET_JOY_ADC_X() == 0 + { PIN_TO_INPUTCTRL(JOY_X_PIN) }, + #endif + #if GET_JOY_ADC_Y() == 0 + { PIN_TO_INPUTCTRL(JOY_Y_PIN) }, + #endif + #if GET_JOY_ADC_Z() == 0 + { PIN_TO_INPUTCTRL(JOY_Z_PIN) }, + #endif + }; + + #define ADC0_AINCOUNT COUNT(adc0_dma_regs_list) + #endif // ADC0_IS_REQUIRED + + #if ADC1_IS_REQUIRED + Adafruit_ZeroDMA adc1DMAProgram, adc1DMARead; + + static constexpr HAL_DMA_DAC_Registers adc1_dma_regs_list[ADC_COUNT] = { + #if GET_TEMP_0_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_0_PIN) }, + #endif + #if GET_TEMP_1_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_1_PIN) }, + #endif + #if GET_TEMP_2_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_2_PIN) }, + #endif + #if GET_TEMP_3_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_3_PIN) }, + #endif + #if GET_TEMP_4_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_4_PIN) }, + #endif + #if GET_TEMP_5_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_5_PIN) }, + #endif + #if GET_TEMP_6_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_6_PIN) }, + #endif + #if GET_TEMP_7_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_7_PIN) }, + #endif + #if GET_BED_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_BED_PIN) }, + #endif + #if GET_CHAMBER_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_CHAMBER_PIN) }, + #endif + #if GET_PROBE_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, + #endif + #if GET_COOLER_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_COOLER_PIN) }, + #endif + #if GET_BOARD_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_BOARD_PIN) }, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 1 + { PIN_TO_INPUTCTRL(FILWIDTH_PIN) }, + #endif + #if GET_BUTTONS_ADC() == 1 + { PIN_TO_INPUTCTRL(ADC_KEYPAD_PIN) }, + #endif + #if GET_JOY_ADC_X() == 1 + { PIN_TO_INPUTCTRL(JOY_X_PIN) }, + #endif + #if GET_JOY_ADC_Y() == 1 + { PIN_TO_INPUTCTRL(JOY_Y_PIN) }, + #endif + #if GET_JOY_ADC_Z() == 1 + { PIN_TO_INPUTCTRL(JOY_Z_PIN) }, + #endif + }; + + #define ADC1_AINCOUNT COUNT(adc1_dma_regs_list) + #endif // ADC1_IS_REQUIRED + +#endif // ADC_IS_REQUIRED + +// ------------------------ +// Private functions +// ------------------------ + +void MarlinHAL::dma_init() { + + #if DMA_IS_REQUIRED + + DmacDescriptor *descriptor; + + #if ADC0_IS_REQUIRED + adc0DMAProgram.setTrigger(ADC0_DMAC_ID_SEQ); + adc0DMAProgram.setAction(DMA_TRIGGER_ACTON_BEAT); + adc0DMAProgram.loop(true); + if (adc0DMAProgram.allocate() == DMA_STATUS_OK) { + descriptor = adc0DMAProgram.addDescriptor( + (void *)adc0_dma_regs_list, // SRC + (void *)&ADC0->DSEQDATA.reg, // DEST + sizeof(adc0_dma_regs_list) / 4, // CNT + DMA_BEAT_SIZE_WORD, + true, // SRCINC + false, // DSTINC + DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE + DMA_STEPSEL_SRC // STEPSEL + ); + if (descriptor) + descriptor->BTCTRL.bit.EVOSEL = DMA_EVENT_OUTPUT_BEAT; + adc0DMAProgram.startJob(); + } + + adc0DMARead.setTrigger(ADC0_DMAC_ID_RESRDY); + adc0DMARead.setAction(DMA_TRIGGER_ACTON_BEAT); + adc0DMARead.loop(true); + if (adc0DMARead.allocate() == DMA_STATUS_OK) { + adc0DMARead.addDescriptor( + (void *)&ADC0->RESULT.reg, // SRC + &adc_results, // DEST + ADC0_AINCOUNT, // CNT + DMA_BEAT_SIZE_HWORD, + false, // SRCINC + true, // DSTINC + DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE + DMA_STEPSEL_DST // STEPSEL + ); + adc0DMARead.startJob(); + } + #endif + #if ADC1_IS_REQUIRED + adc1DMAProgram.setTrigger(ADC1_DMAC_ID_SEQ); + adc1DMAProgram.setAction(DMA_TRIGGER_ACTON_BEAT); + adc1DMAProgram.loop(true); + if (adc1DMAProgram.allocate() == DMA_STATUS_OK) { + descriptor = adc1DMAProgram.addDescriptor( + (void *)adc1_dma_regs_list, // SRC + (void *)&ADC1->DSEQDATA.reg, // DEST + sizeof(adc1_dma_regs_list) / 4, // CNT + DMA_BEAT_SIZE_WORD, + true, // SRCINC + false, // DSTINC + DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE + DMA_STEPSEL_SRC // STEPSEL + ); + if (descriptor) + descriptor->BTCTRL.bit.EVOSEL = DMA_EVENT_OUTPUT_BEAT; + adc1DMAProgram.startJob(); + } + + adc1DMARead.setTrigger(ADC1_DMAC_ID_RESRDY); + adc1DMARead.setAction(DMA_TRIGGER_ACTON_BEAT); + adc1DMARead.loop(true); + if (adc1DMARead.allocate() == DMA_STATUS_OK) { + adc1DMARead.addDescriptor( + (void *)&ADC1->RESULT.reg, // SRC + &adc_results[ADC0_AINCOUNT], // DEST + ADC1_AINCOUNT, // CNT + DMA_BEAT_SIZE_HWORD, + false, // SRCINC + true, // DSTINC + DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE + DMA_STEPSEL_DST // STEPSEL + ); + adc1DMARead.startJob(); + } + #endif + + DMAC->PRICTRL0.bit.RRLVLEN0 = true; // Activate round robin for DMA channels required by ADCs + + #endif // DMA_IS_REQUIRED +} + +// ------------------------ +// Public functions +// ------------------------ + +// HAL initialization task +void MarlinHAL::init() { + TERN_(DMA_IS_REQUIRED, dma_init()); + #if ENABLED(SDSUPPORT) + #if HAS_SD_DETECT && SD_CONNECTION_IS(ONBOARD) + SET_INPUT_PULLUP(SD_DETECT_PIN); + #endif + OUT_WRITE(SDSS, HIGH); // Try to set SDSS inactive before any other SPI users start up + #endif +} + +#pragma push_macro("WDT") +#undef WDT // Required to be able to use '.bit.WDT'. Compiler wrongly replace struct field with WDT define +uint8_t MarlinHAL::get_reset_source() { + RSTC_RCAUSE_Type resetCause; + + resetCause.reg = REG_RSTC_RCAUSE; + if (resetCause.bit.POR) return RST_POWER_ON; + else if (resetCause.bit.EXT) return RST_EXTERNAL; + else if (resetCause.bit.BODCORE || resetCause.bit.BODVDD) return RST_BROWN_OUT; + else if (resetCause.bit.WDT) return RST_WATCHDOG; + else if (resetCause.bit.SYST || resetCause.bit.NVM) return RST_SOFTWARE; + else if (resetCause.bit.BACKUP) return RST_BACKUP; + return 0; +} +#pragma pop_macro("WDT") + +void MarlinHAL::reboot() { NVIC_SystemReset(); } + +extern "C" { + void * _sbrk(int incr); + + extern unsigned int __bss_end__; // end of bss section +} + +// Return free memory between end of heap (or end bss) and whatever is current +int freeMemory() { + int free_memory, heap_end = (int)_sbrk(0); + return (int)&free_memory - (heap_end ?: (int)&__bss_end__); +} + +// ------------------------ +// ADC +// ------------------------ + +uint16_t MarlinHAL::adc_result; + +void MarlinHAL::adc_init() { + #if ADC_IS_REQUIRED + memset(adc_results, 0xFF, sizeof(adc_results)); // Fill result with invalid values + + LOOP_L_N(pi, COUNT(adc_pins)) + pinPeripheral(adc_pins[pi], PIO_ANALOG); + + LOOP_S_LE_N(ai, FIRST_ADC, LAST_ADC) { + Adc* adc = ((Adc*[])ADC_INSTS)[ai]; + + // ADC clock setup + GCLK->PCHCTRL[ADC0_GCLK_ID + ai].bit.CHEN = false; + SYNC(GCLK->PCHCTRL[ADC0_GCLK_ID + ai].bit.CHEN); + GCLK->PCHCTRL[ADC0_GCLK_ID + ai].reg = GCLK_PCHCTRL_GEN_GCLK1 | GCLK_PCHCTRL_CHEN; // 48MHz startup code programmed + SYNC(!GCLK->PCHCTRL[ADC0_GCLK_ID + ai].bit.CHEN); + adc->CTRLA.bit.PRESCALER = ADC_CTRLA_PRESCALER_DIV32_Val; // 1.5MHZ adc clock + + // ADC setup + // Preloaded data (fixed for all ADC instances hence not loaded by DMA) + adc->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA_Val; // VRefA pin + SYNC(adc->SYNCBUSY.bit.REFCTRL); + adc->CTRLB.bit.RESSEL = ADC_CTRLB_RESSEL_10BIT_Val; // ... ADC_CTRLB_RESSEL_16BIT_Val + SYNC(adc->SYNCBUSY.bit.CTRLB); + adc->SAMPCTRL.bit.SAMPLEN = (6 - 1); // Sampling clocks + //adc->AVGCTRL.reg = ADC_AVGCTRL_SAMPLENUM_16 | ADC_AVGCTRL_ADJRES(4); // 16 Accumulated conversions and shift 4 to get oversampled 12 bits result + //SYNC(adc->SYNCBUSY.bit.AVGCTRL); + + // Registers loaded by DMA + adc->DSEQCTRL.bit.INPUTCTRL = true; + adc->DSEQCTRL.bit.AUTOSTART = true; // Start conversion after DMA sequence + + adc->CTRLA.bit.ENABLE = true; // Enable ADC + SYNC(adc->SYNCBUSY.bit.ENABLE); + } + #endif // ADC_IS_REQUIRED +} + +void MarlinHAL::adc_start(const pin_t pin) { + #if ADC_IS_REQUIRED + LOOP_L_N(pi, COUNT(adc_pins)) + if (pin == adc_pins[pi]) { adc_result = adc_results[pi]; return; } + #endif + + adc_result = 0xFFFF; +} + +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/HAL.h b/src/HAL/SAMD51/HAL.h new file mode 100644 index 0000000..79ba802 --- /dev/null +++ b/src/HAL/SAMD51/HAL.h @@ -0,0 +1,216 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define CPU_32_BIT + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" + +#ifdef ADAFRUIT_GRAND_CENTRAL_M4 + #include "MarlinSerial_AGCM4.h" + + // Serial ports + typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; + typedef ForwardSerial1Class< decltype(Serial1) > DefaultSerial2; + typedef ForwardSerial1Class< decltype(Serial2) > DefaultSerial3; + typedef ForwardSerial1Class< decltype(Serial3) > DefaultSerial4; + typedef ForwardSerial1Class< decltype(Serial4) > DefaultSerial5; + extern DefaultSerial1 MSerial0; + extern DefaultSerial2 MSerial1; + extern DefaultSerial3 MSerial2; + extern DefaultSerial4 MSerial3; + extern DefaultSerial5 MSerial4; + + #define __MSERIAL(X) MSerial##X + #define _MSERIAL(X) __MSERIAL(X) + #define MSERIAL(X) _MSERIAL(INCREMENT(X)) + + #if SERIAL_PORT == -1 + #define MYSERIAL1 MSerial0 + #elif WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) + #else + #error "SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + + #ifdef SERIAL_PORT_2 + #if SERIAL_PORT_2 == -1 + #define MYSERIAL2 MSerial0 + #elif WITHIN(SERIAL_PORT_2, 0, 3) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + #endif + + #ifdef MMU2_SERIAL_PORT + #if MMU2_SERIAL_PORT == -1 + #define MMU2_SERIAL MSerial0 + #elif WITHIN(MMU2_SERIAL_PORT, 0, 3) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #error "MMU2_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + #endif + + #ifdef LCD_SERIAL_PORT + #if LCD_SERIAL_PORT == -1 + #define LCD_SERIAL MSerial0 + #elif WITHIN(LCD_SERIAL_PORT, 0, 3) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #error "LCD_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + #endif + +#endif // ADAFRUIT_GRAND_CENTRAL_M4 + +typedef int8_t pin_t; + +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; + +// +// Interrupts +// +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() + +#define cli() __disable_irq() // Disable interrupts +#define sei() __enable_irq() // Enable interrupts + +// +// ADC +// + +//#define HAL_ADC_FILTERED // Disable Marlin's oversampling. The HAL filters ADC values. +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 // ... 12 + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +// +// Tone +// +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); +void noTone(const pin_t _pin); + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +#ifdef __cplusplus + extern "C" { +#endif + +char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s); + +extern "C" int freeMemory(); + +#ifdef __cplusplus + } +#endif + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) {} + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +private: + static void dma_init(); +}; diff --git a/src/HAL/SAMD51/HAL_SPI.cpp b/src/HAL/SAMD51/HAL_SPI.cpp new file mode 100644 index 0000000..77f4d5e --- /dev/null +++ b/src/HAL/SAMD51/HAL_SPI.cpp @@ -0,0 +1,148 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Hardware and software SPI implementations are included in this file. + * + * Control of the slave select pin(s) is handled by the calling routines and + * SAMD51 let hardware SPI handling to remove SS from its logic. + */ + +#ifdef __SAMD51__ + +// -------------------------------------------------------------------------- +// Includes +// -------------------------------------------------------------------------- + +#include "../../inc/MarlinConfig.h" +#include + +// -------------------------------------------------------------------------- +// Public functions +// -------------------------------------------------------------------------- + +#if EITHER(SOFTWARE_SPI, FORCE_SOFT_SPI) + + // ------------------------ + // Software SPI + // ------------------------ + #error "Software SPI not supported for SAMD51. Use Hardware SPI." + +#else // !SOFTWARE_SPI + + #ifdef ADAFRUIT_GRAND_CENTRAL_M4 + #if SD_CONNECTION_IS(ONBOARD) + #define sdSPI SDCARD_SPI + #else + #define sdSPI SPI + #endif + #endif + + static SPISettings spiConfig; + + // ------------------------ + // Hardware SPI + // ------------------------ + void spiBegin() { + spiInit(SPI_HALF_SPEED); + } + + void spiInit(uint8_t spiRate) { + // Use datarates Marlin uses + uint32_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = 8000000; break; + case SPI_HALF_SPEED: clock = 4000000; break; + case SPI_QUARTER_SPEED: clock = 2000000; break; + case SPI_EIGHTH_SPEED: clock = 1000000; break; + case SPI_SIXTEENTH_SPEED: clock = 500000; break; + case SPI_SPEED_5: clock = 250000; break; + case SPI_SPEED_6: clock = 125000; break; + default: clock = 4000000; break; // Default from the SPI library + } + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + sdSPI.begin(); + } + + /** + * @brief Receives a single byte from the SPI port. + * + * @return Byte received + * + * @details + */ + uint8_t spiRec() { + sdSPI.beginTransaction(spiConfig); + uint8_t returnByte = sdSPI.transfer(0xFF); + sdSPI.endTransaction(); + return returnByte; + } + + /** + * @brief Receives a number of bytes from the SPI port to a buffer + * + * @param buf Pointer to starting address of buffer to write to. + * @param nbyte Number of bytes to receive. + * @return Nothing + */ + void spiRead(uint8_t *buf, uint16_t nbyte) { + if (nbyte == 0) return; + memset(buf, 0xFF, nbyte); + sdSPI.beginTransaction(spiConfig); + sdSPI.transfer(buf, nbyte); + sdSPI.endTransaction(); + } + + /** + * @brief Sends a single byte on SPI port + * + * @param b Byte to send + * + * @details + */ + void spiSend(uint8_t b) { + sdSPI.beginTransaction(spiConfig); + sdSPI.transfer(b); + sdSPI.endTransaction(); + } + + /** + * @brief Write token and then write from 512 byte buffer to SPI (for SD card) + * + * @param buf Pointer with buffer start address + * @return Nothing + * + * @details Uses DMA + */ + void spiSendBlock(uint8_t token, const uint8_t *buf) { + sdSPI.beginTransaction(spiConfig); + sdSPI.transfer(token); + sdSPI.transfer((uint8_t*)buf, nullptr, 512); + sdSPI.endTransaction(); + } + + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + spiConfig = SPISettings(spiClock, (BitOrder)bitOrder, dataMode); + sdSPI.beginTransaction(spiConfig); + } +#endif // !SOFTWARE_SPI + +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/MarlinSPI.h b/src/HAL/SAMD51/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/SAMD51/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp b/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp new file mode 100644 index 0000000..a16ea2f --- /dev/null +++ b/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ADAFRUIT_GRAND_CENTRAL_M4 + +/** + * Framework doesn't define some serials to save sercom resources + * hence if these are used I need to define them + */ + +#include "../../inc/MarlinConfig.h" + +#if USING_HW_SERIAL1 + UartT Serial2(false, &sercom4, PIN_SERIAL2_RX, PIN_SERIAL2_TX, PAD_SERIAL2_RX, PAD_SERIAL2_TX); + void SERCOM4_0_Handler() { Serial2.IrqHandler(); } + void SERCOM4_1_Handler() { Serial2.IrqHandler(); } + void SERCOM4_2_Handler() { Serial2.IrqHandler(); } + void SERCOM4_3_Handler() { Serial2.IrqHandler(); } +#endif + +#if USING_HW_SERIAL2 + UartT Serial3(false, &sercom1, PIN_SERIAL3_RX, PIN_SERIAL3_TX, PAD_SERIAL3_RX, PAD_SERIAL3_TX); + void SERCOM1_0_Handler() { Serial3.IrqHandler(); } + void SERCOM1_1_Handler() { Serial3.IrqHandler(); } + void SERCOM1_2_Handler() { Serial3.IrqHandler(); } + void SERCOM1_3_Handler() { Serial3.IrqHandler(); } +#endif + +#if USING_HW_SERIAL3 + UartT Serial4(false, &sercom5, PIN_SERIAL4_RX, PIN_SERIAL4_TX, PAD_SERIAL4_RX, PAD_SERIAL4_TX); + void SERCOM5_0_Handler() { Serial4.IrqHandler(); } + void SERCOM5_1_Handler() { Serial4.IrqHandler(); } + void SERCOM5_2_Handler() { Serial4.IrqHandler(); } + void SERCOM5_3_Handler() { Serial4.IrqHandler(); } +#endif + +#endif // ADAFRUIT_GRAND_CENTRAL_M4 diff --git a/src/HAL/SAMD51/MarlinSerial_AGCM4.h b/src/HAL/SAMD51/MarlinSerial_AGCM4.h new file mode 100644 index 0000000..ac5a379 --- /dev/null +++ b/src/HAL/SAMD51/MarlinSerial_AGCM4.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/serial_hook.h" + +typedef Serial1Class UartT; + +extern UartT Serial2; +extern UartT Serial3; +extern UartT Serial4; diff --git a/src/HAL/SAMD51/QSPIFlash.cpp b/src/HAL/SAMD51/QSPIFlash.cpp new file mode 100644 index 0000000..fc21a1a --- /dev/null +++ b/src/HAL/SAMD51/QSPIFlash.cpp @@ -0,0 +1,78 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(QSPI_EEPROM) + +#include "QSPIFlash.h" + +#define INVALID_ADDR 0xFFFFFFFF +#define SECTOR_OF(a) (a & ~(SFLASH_SECTOR_SIZE - 1)) +#define OFFSET_OF(a) (a & (SFLASH_SECTOR_SIZE - 1)) + +Adafruit_SPIFlashBase * QSPIFlash::_flashBase = nullptr; +uint8_t QSPIFlash::_buf[SFLASH_SECTOR_SIZE]; +uint32_t QSPIFlash::_addr = INVALID_ADDR; + +void QSPIFlash::begin() { + if (_flashBase) return; + + _flashBase = new Adafruit_SPIFlashBase(new Adafruit_FlashTransport_QSPI()); + _flashBase->begin(nullptr); +} + +size_t QSPIFlash::size() { + return _flashBase->size(); +} + +uint8_t QSPIFlash::readByte(const uint32_t address) { + if (SECTOR_OF(address) == _addr) return _buf[OFFSET_OF(address)]; + + return _flashBase->read8(address); +} + +void QSPIFlash::writeByte(const uint32_t address, const uint8_t value) { + uint32_t const sector_addr = SECTOR_OF(address); + + // Page changes, flush old and update new cache + if (sector_addr != _addr) { + flush(); + _addr = sector_addr; + + // read a whole page from flash + _flashBase->readBuffer(sector_addr, _buf, SFLASH_SECTOR_SIZE); + } + + _buf[OFFSET_OF(address)] = value; +} + +void QSPIFlash::flush() { + if (_addr == INVALID_ADDR) return; + + _flashBase->eraseSector(_addr / SFLASH_SECTOR_SIZE); + _flashBase->writeBuffer(_addr, _buf, SFLASH_SECTOR_SIZE); + + _addr = INVALID_ADDR; +} + +#endif // QSPI_EEPROM diff --git a/src/HAL/SAMD51/QSPIFlash.h b/src/HAL/SAMD51/QSPIFlash.h new file mode 100644 index 0000000..58822fe --- /dev/null +++ b/src/HAL/SAMD51/QSPIFlash.h @@ -0,0 +1,49 @@ +/** + * @file QSPIFlash.h + * + * The MIT License (MIT) + * + * Copyright (c) 2019 Ha Thach and Dean Miller for Adafruit Industries LLC + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + * Derived from Adafruit_SPIFlash class with no SdFat references + */ +#pragma once + +#include + +// This class extends Adafruit_SPIFlashBase by adding caching support. +// +// This class will use 4096 Bytes of RAM as a block cache. +class QSPIFlash { + public: + static void begin(); + static size_t size(); + static uint8_t readByte(const uint32_t address); + static void writeByte(const uint32_t address, const uint8_t v); + static void flush(); + + private: + static Adafruit_SPIFlashBase * _flashBase; + static uint8_t _buf[SFLASH_SECTOR_SIZE]; + static uint32_t _addr; +}; + +extern QSPIFlash qspi; diff --git a/src/HAL/SAMD51/SAMD51.h b/src/HAL/SAMD51/SAMD51.h new file mode 100644 index 0000000..7839561 --- /dev/null +++ b/src/HAL/SAMD51/SAMD51.h @@ -0,0 +1,70 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define SYNC(sc) while (sc) { \ + asm(""); \ + } + +// Get SAMD port/pin from specified arduino pin +#define GET_SAMD_PORT(P) _GET_SAMD_PORT(PIN_TO_SAMD_PIN(P)) +#define GET_SAMD_PIN(P) _GET_SAMD_PIN(PIN_TO_SAMD_PIN(P)) + +// Get external interrupt line associated to specified arduino pin +#define PIN_TO_EILINE(P) _SAMDPORTPIN_TO_EILINE(GET_SAMD_PORT(P), GET_SAMD_PIN(P)) + +// Get adc/ain associated to specified arduino pin +#define PIN_TO_ADC(P) (ANAPIN_TO_ADCAIN(P) >> 8) +#define PIN_TO_AIN(P) (ANAPIN_TO_ADCAIN(P) & 0xFF) + +// Private defines +#define PIN_TO_SAMD_PIN(P) DIO##P##_PIN + +#define _GET_SAMD_PORT(P) ((P) >> 5) +#define _GET_SAMD_PIN(P) ((P) & 0x1F) + +// Get external interrupt line +#define _SAMDPORTPIN_TO_EILINE(P,B) ((P == 0 && WITHIN(B, 0, 31) && B != 8 && B != 26 && B != 28 && B != 29) ? (B) & 0xF \ + : (P == 1 && (WITHIN(B, 0, 25) || WITHIN(B, 30, 31))) ? (B) & 0xF \ + : (P == 1 && WITHIN(B, 26, 29)) ? 12 + (B) - 26 \ + : (P == 2 && (WITHIN(B, 0, 6) || WITHIN(B, 10, 31)) && B != 29) ? (B) & 0xF \ + : (P == 2 && B == 7) ? 9 \ + : (P == 3 && WITHIN(B, 0, 1)) ? (B) \ + : (P == 3 && WITHIN(B, 8, 12)) ? 3 + (B) - 8 \ + : (P == 3 && WITHIN(B, 20, 21)) ? 10 + (B) - 20 \ + : -1) + +// Get adc/ain +#define ANAPIN_TO_ADCAIN(P) _PIN_TO_ADCAIN(ANAPIN_TO_SAMDPIN(P)) +#define _PIN_TO_ADCAIN(P) _SAMDPORTPIN_TO_ADCAIN(_GET_SAMD_PORT(P), _GET_SAMD_PIN(P)) + +#define _SAMDPORTPIN_TO_ADCAIN(P,B) ((P == 0 && WITHIN(B, 2, 3)) ? 0x000 + (B) - 2 \ + : (P == 0 && WITHIN(B, 4, 7)) ? 0x000 + (B) \ + : (P == 0 && WITHIN(B, 8, 9)) ? 0x100 + 2 + (B) - 8 \ + : (P == 0 && WITHIN(B, 10, 11)) ? 0x000 + (B) \ + : (P == 1 && WITHIN(B, 0, 3)) ? 0x000 + 12 + (B) \ + : (P == 1 && WITHIN(B, 4, 7)) ? 0x100 + 6 + (B) - 4 \ + : (P == 1 && WITHIN(B, 8, 9)) ? 0x100 + (B) - 8 \ + : (P == 2 && WITHIN(B, 0, 1)) ? 0x100 + 10 + (B) \ + : (P == 2 && WITHIN(B, 2, 3)) ? 0x100 + 4 + (B) - 2 \ + : (P == 2 && WITHIN(B, 30, 31)) ? 0x100 + 12 + (B) - 30 \ + : (P == 3 && WITHIN(B, 0, 1)) ? 0x100 + 14 + (B) \ + : -1) diff --git a/src/HAL/SAMD51/Servo.cpp b/src/HAL/SAMD51/Servo.cpp new file mode 100644 index 0000000..665322f --- /dev/null +++ b/src/HAL/SAMD51/Servo.cpp @@ -0,0 +1,217 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * This comes from Arduino library which at the moment is buggy and uncompilable + */ + +#ifdef __SAMD51__ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "../shared/servo.h" +#include "../shared/servo_private.h" +#include "SAMD51.h" + +#define __TC_GCLK_ID(t) TC##t##_GCLK_ID +#define _TC_GCLK_ID(t) __TC_GCLK_ID(t) +#define TC_GCLK_ID _TC_GCLK_ID(SERVO_TC) + +#define _TC_PRESCALER(d) TC_CTRLA_PRESCALER_DIV##d##_Val +#define TC_PRESCALER(d) _TC_PRESCALER(d) + +#define __SERVO_IRQn(t) TC##t##_IRQn +#define _SERVO_IRQn(t) __SERVO_IRQn(t) +#define SERVO_IRQn _SERVO_IRQn(SERVO_TC) + +#define HAL_SERVO_TIMER_ISR() TC_HANDLER(SERVO_TC) + +#define TIMER_TCCHANNEL(t) ((t) & 1) +#define TC_COUNTER_START_VAL 0xFFFF + + +static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval) + +FORCE_INLINE static uint16_t getTimerCount() { + Tc * const tc = timer_config[SERVO_TC].pTc; + + tc->COUNT16.CTRLBSET.reg = TC_CTRLBCLR_CMD_READSYNC; + SYNC(tc->COUNT16.SYNCBUSY.bit.CTRLB || tc->COUNT16.SYNCBUSY.bit.COUNT); + + return tc->COUNT16.COUNT.reg; +} + +// ---------------------------- +// Interrupt handler for the TC +// ---------------------------- +HAL_SERVO_TIMER_ISR() { + Tc * const tc = timer_config[SERVO_TC].pTc; + const timer16_Sequence_t timer = + #ifndef _useTimer1 + _timer2 + #elif !defined(_useTimer2) + _timer1 + #else + (tc->COUNT16.INTFLAG.reg & tc->COUNT16.INTENSET.reg & TC_INTFLAG_MC0) ? _timer1 : _timer2 + #endif + ; + const uint8_t tcChannel = TIMER_TCCHANNEL(timer); + + int8_t cho = currentServoIndex[timer]; // Handle the prior servo first + if (cho < 0) { // Servo -1 indicates the refresh interval completed... + #if defined(_useTimer1) && defined(_useTimer2) + if (currentServoIndex[timer ^ 1] >= 0) { + // Wait for both channels + // Clear the interrupt + tc->COUNT16.INTFLAG.reg = (tcChannel == 0) ? TC_INTFLAG_MC0 : TC_INTFLAG_MC1; + return; + } + #endif + tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer + SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT); + } + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? + digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW + + currentServoIndex[timer] = ++cho; // go to the next channel (or 0) + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { + if (SERVO(timer, cho).Pin.isActive) // activated? + digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH + + tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks; + } + else { + // finished all channels so wait for the refresh period to expire before starting over + currentServoIndex[timer] = -1; // reset the timer COUNT.reg on the next call + const uint16_t cval = getTimerCount() - 256 / (SERVO_TIMER_PRESCALER), // allow 256 cycles to ensure the next CV not missed + ival = (TC_COUNTER_START_VAL) - (uint16_t)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed + tc->COUNT16.CC[tcChannel].reg = min(cval, ival); + } + if (tcChannel == 0) { + SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); + tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt + } + else { + SYNC(tc->COUNT16.SYNCBUSY.bit.CC1); + tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt + } +} + +void initISR(const timer16_Sequence_t timer) { + Tc * const tc = timer_config[SERVO_TC].pTc; + const uint8_t tcChannel = TIMER_TCCHANNEL(timer); + + static bool initialized = false; // Servo TC has been initialized + if (!initialized) { + NVIC_DisableIRQ(SERVO_IRQn); + + // Disable the timer + tc->COUNT16.CTRLA.bit.ENABLE = false; + SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE); + + // Select GCLK0 as timer/counter input clock source + GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN = false; + SYNC(GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN); + GCLK->PCHCTRL[TC_GCLK_ID].reg = GCLK_PCHCTRL_GEN_GCLK0 | GCLK_PCHCTRL_CHEN; // 120MHz startup code programmed + SYNC(!GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN); + + // Reset the timer + tc->COUNT16.CTRLA.bit.SWRST = true; + SYNC(tc->COUNT16.SYNCBUSY.bit.SWRST); + SYNC(tc->COUNT16.CTRLA.bit.SWRST); + + // Set timer counter mode to 16 bits + tc->COUNT16.CTRLA.reg = TC_CTRLA_MODE_COUNT16; + + // Set timer counter mode as normal PWM + tc->COUNT16.WAVE.bit.WAVEGEN = TCC_WAVE_WAVEGEN_NPWM_Val; + + // Set the prescaler factor + tc->COUNT16.CTRLA.bit.PRESCALER = TC_PRESCALER(SERVO_TIMER_PRESCALER); + + // Count down + tc->COUNT16.CTRLBSET.reg = TC_CTRLBCLR_DIR; + SYNC(tc->COUNT16.SYNCBUSY.bit.CTRLB); + + // Reset all servo indexes + memset((void *)currentServoIndex, 0xFF, sizeof(currentServoIndex)); + + // Configure interrupt request + NVIC_ClearPendingIRQ(SERVO_IRQn); + NVIC_SetPriority(SERVO_IRQn, 5); + NVIC_EnableIRQ(SERVO_IRQn); + + initialized = true; + } + + if (!tc->COUNT16.CTRLA.bit.ENABLE) { + // Reset the timer counter + tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; + SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT); + + // Enable the timer and start it + tc->COUNT16.CTRLA.bit.ENABLE = true; + SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE); + } + // First interrupt request after 1 ms + tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)usToTicks(1000UL); + + if (tcChannel == 0 ) { + SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); + + // Clear pending match interrupt + tc->COUNT16.INTFLAG.reg = TC_INTENSET_MC0; + // Enable the match channel interrupt request + tc->COUNT16.INTENSET.reg = TC_INTENSET_MC0; + } + else { + SYNC(tc->COUNT16.SYNCBUSY.bit.CC1); + + // Clear pending match interrupt + tc->COUNT16.INTFLAG.reg = TC_INTENSET_MC1; + // Enable the match channel interrupt request + tc->COUNT16.INTENSET.reg = TC_INTENSET_MC1; + } +} + +void finISR(const timer16_Sequence_t timer_index) { + Tc * const tc = timer_config[SERVO_TC].pTc; + const uint8_t tcChannel = TIMER_TCCHANNEL(timer_index); + + // Disable the match channel interrupt request + tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1; + + if (true + #if defined(_useTimer1) && defined(_useTimer2) + && (tc->COUNT16.INTENCLR.reg & (TC_INTENCLR_MC0|TC_INTENCLR_MC1)) == 0 + #endif + ) { + // Disable the timer if not used + tc->COUNT16.CTRLA.bit.ENABLE = false; + SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE); + } +} + +#endif // HAS_SERVOS + +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/ServoTimers.h b/src/HAL/SAMD51/ServoTimers.h new file mode 100644 index 0000000..948d515 --- /dev/null +++ b/src/HAL/SAMD51/ServoTimers.h @@ -0,0 +1,39 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define _useTimer1 +#define _useTimer2 + +#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays +#define SERVO_TIMER_PRESCALER 64 // timer prescaler factor to 64 (avoid overflowing 16-bit clock counter, at 120MHz this is 1831 ticks per millisecond + +#define SERVO_TC 3 + +typedef enum { + #ifdef _useTimer1 + _timer1, + #endif + #ifdef _useTimer2 + _timer2, + #endif + _Nbr_16timers +} timer16_Sequence_t; diff --git a/src/HAL/SAMD51/eeprom_flash.cpp b/src/HAL/SAMD51/eeprom_flash.cpp new file mode 100644 index 0000000..871bf22 --- /dev/null +++ b/src/HAL/SAMD51/eeprom_flash.cpp @@ -0,0 +1,95 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __SAMD51__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FLASH_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" + +#define NVMCTRL_CMD(c) do{ \ + SYNC(!NVMCTRL->STATUS.bit.READY); \ + NVMCTRL->INTFLAG.bit.DONE = true; \ + NVMCTRL->CTRLB.reg = c | NVMCTRL_CTRLB_CMDEX_KEY; \ + SYNC(NVMCTRL->INTFLAG.bit.DONE); \ + }while(0) +#define NVMCTRL_FLUSH() do{ \ + if (NVMCTRL->SEESTAT.bit.LOAD) \ + NVMCTRL_CMD(NVMCTRL_CTRLB_CMD_SEEFLUSH); \ + }while(0) + +size_t PersistentStore::capacity() { + const uint8_t psz = NVMCTRL->SEESTAT.bit.PSZ, + sblk = NVMCTRL->SEESTAT.bit.SBLK; + + return (!psz && !sblk) ? 0 + : (psz <= 2) ? (0x200 << psz) + : (sblk == 1 || psz == 3) ? 4096 + : (sblk == 2 || psz == 4) ? 8192 + : (sblk <= 4 || psz == 5) ? 16384 + : (sblk >= 9 && psz == 7) ? 65536 + : 32768; +} + +bool PersistentStore::access_start() { + NVMCTRL->SEECFG.reg = NVMCTRL_SEECFG_WMODE_BUFFERED; // Buffered mode and segment reallocation active + if (NVMCTRL->SEESTAT.bit.RLOCK) + NVMCTRL_CMD(NVMCTRL_CTRLB_CMD_USEE); // Unlock E2P data write access + return true; +} + +bool PersistentStore::access_finish() { + NVMCTRL_FLUSH(); + if (!NVMCTRL->SEESTAT.bit.LOCK) + NVMCTRL_CMD(NVMCTRL_CTRLB_CMD_LSEE); // Lock E2P data write access + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + while (size--) { + const uint8_t v = *value; + SYNC(NVMCTRL->SEESTAT.bit.BUSY); + if (NVMCTRL->INTFLAG.bit.SEESFULL) + NVMCTRL_FLUSH(); // Next write will trigger a sector reallocation. I need to flush 'pagebuffer' + ((volatile uint8_t *)SEEPROM_ADDR)[pos] = v; + SYNC(!NVMCTRL->INTFLAG.bit.SEEWRC); + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + while (size--) { + SYNC(NVMCTRL->SEESTAT.bit.BUSY); + uint8_t c = ((volatile uint8_t *)SEEPROM_ADDR)[pos]; + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } + return false; +} + +#endif // FLASH_EEPROM_EMULATION +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/eeprom_qspi.cpp b/src/HAL/SAMD51/eeprom_qspi.cpp new file mode 100644 index 0000000..faa7637 --- /dev/null +++ b/src/HAL/SAMD51/eeprom_qspi.cpp @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __SAMD51__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(QSPI_EEPROM) + +#include "../shared/eeprom_api.h" + +#include "QSPIFlash.h" + +static bool initialized; + +size_t PersistentStore::capacity() { return qspi.size(); } + +bool PersistentStore::access_start() { + if (!initialized) { + qspi.begin(); + initialized = true; + } + return true; +} + +bool PersistentStore::access_finish() { + qspi.flush(); + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + while (size--) { + const uint8_t v = *value; + qspi.writeByte(pos, v); + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + while (size--) { + uint8_t c = qspi.readByte(pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } + return false; +} + +#endif // QSPI_EEPROM +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/eeprom_wired.cpp b/src/HAL/SAMD51/eeprom_wired.cpp new file mode 100644 index 0000000..3481fe5 --- /dev/null +++ b/src/HAL/SAMD51/eeprom_wired.cpp @@ -0,0 +1,75 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __SAMD51__ + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for I2C / SPI EEPROM." +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + const uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + while (size--) { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/endstop_interrupts.h b/src/HAL/SAMD51/endstop_interrupts.h new file mode 100644 index 0000000..61a06c0 --- /dev/null +++ b/src/HAL/SAMD51/endstop_interrupts.h @@ -0,0 +1,202 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop interrupts for ATMEL SAMD51 based targets. + * + * On SAMD51, all pins support external interrupt capability. + * Any pin can be used for external interrupts, but there are some restrictions. + * At most 16 different external interrupts can be used at one time. + * Further, you can’t just pick any 16 pins to use. This is because every pin on the SAMD51 + * connects to what is called an EXTINT line, and only one pin per EXTINT line can be used for external + * interrupts at a time + */ + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +#define MATCH_EILINE(P1,P2) (P1 != P2 && PIN_TO_EILINE(P1) == PIN_TO_EILINE(P2)) +#define MATCH_X_MAX_EILINE(P) TERN0(HAS_X_MAX, DEFER4(MATCH_EILINE)(P, X_MAX_PIN)) +#define MATCH_X_MIN_EILINE(P) TERN0(HAS_X_MIN, DEFER4(MATCH_EILINE)(P, X_MIN_PIN)) +#define MATCH_Y_MAX_EILINE(P) TERN0(HAS_Y_MAX, DEFER4(MATCH_EILINE)(P, Y_MAX_PIN)) +#define MATCH_Y_MIN_EILINE(P) TERN0(HAS_Y_MIN, DEFER4(MATCH_EILINE)(P, Y_MIN_PIN)) +#define MATCH_Z_MAX_EILINE(P) TERN0(HAS_Z_MAX, DEFER4(MATCH_EILINE)(P, Z_MAX_PIN)) +#define MATCH_Z_MIN_EILINE(P) TERN0(HAS_Z_MIN, DEFER4(MATCH_EILINE)(P, Z_MIN_PIN)) +#define MATCH_I_MAX_EILINE(P) TERN0(HAS_I_MAX, DEFER4(MATCH_EILINE)(P, I_MAX_PIN)) +#define MATCH_I_MIN_EILINE(P) TERN0(HAS_I_MIN, DEFER4(MATCH_EILINE)(P, I_MIN_PIN)) +#define MATCH_J_MAX_EILINE(P) TERN0(HAS_J_MAX, DEFER4(MATCH_EILINE)(P, J_MAX_PIN)) +#define MATCH_J_MIN_EILINE(P) TERN0(HAS_J_MIN, DEFER4(MATCH_EILINE)(P, J_MIN_PIN)) +#define MATCH_K_MAX_EILINE(P) TERN0(HAS_K_MAX, DEFER4(MATCH_EILINE)(P, K_MAX_PIN)) +#define MATCH_K_MIN_EILINE(P) TERN0(HAS_K_MIN, DEFER4(MATCH_EILINE)(P, K_MIN_PIN)) +#define MATCH_Z2_MAX_EILINE(P) TERN0(HAS_Z2_MAX, DEFER4(MATCH_EILINE)(P, Z2_MAX_PIN)) +#define MATCH_Z2_MIN_EILINE(P) TERN0(HAS_Z2_MIN, DEFER4(MATCH_EILINE)(P, Z2_MIN_PIN)) +#define MATCH_Z3_MAX_EILINE(P) TERN0(HAS_Z3_MAX, DEFER4(MATCH_EILINE)(P, Z3_MAX_PIN)) +#define MATCH_Z3_MIN_EILINE(P) TERN0(HAS_Z3_MIN, DEFER4(MATCH_EILINE)(P, Z3_MIN_PIN)) +#define MATCH_Z4_MAX_EILINE(P) TERN0(HAS_Z4_MAX, DEFER4(MATCH_EILINE)(P, Z4_MAX_PIN)) +#define MATCH_Z4_MIN_EILINE(P) TERN0(HAS_Z4_MIN, DEFER4(MATCH_EILINE)(P, Z4_MIN_PIN)) +#define MATCH_Z_MIN_PROBE_EILINE(P) TERN0(HAS_Z_MIN_PROBE_PIN, DEFER4(MATCH_EILINE)(P, Z_MIN_PROBE_PIN)) + +#define AVAILABLE_EILINE(P) ( PIN_TO_EILINE(P) != -1 \ + && !MATCH_X_MAX_EILINE(P) && !MATCH_X_MIN_EILINE(P) \ + && !MATCH_Y_MAX_EILINE(P) && !MATCH_Y_MIN_EILINE(P) \ + && !MATCH_Z_MAX_EILINE(P) && !MATCH_Z_MIN_EILINE(P) \ + && !MATCH_I_MAX_EILINE(P) && !MATCH_I_MIN_EILINE(P) \ + && !MATCH_J_MAX_EILINE(P) && !MATCH_J_MIN_EILINE(P) \ + && !MATCH_K_MAX_EILINE(P) && !MATCH_K_MIN_EILINE(P) \ + && !MATCH_Z2_MAX_EILINE(P) && !MATCH_Z2_MIN_EILINE(P) \ + && !MATCH_Z3_MAX_EILINE(P) && !MATCH_Z3_MIN_EILINE(P) \ + && !MATCH_Z4_MAX_EILINE(P) && !MATCH_Z4_MIN_EILINE(P) \ + && !MATCH_Z_MIN_PROBE_EILINE(P) ) + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(P, endstop_ISR, CHANGE) + #if HAS_X_MAX + #if !AVAILABLE_EILINE(X_MAX_PIN) + #error "X_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(X_MAX_PIN); + #endif + #if HAS_X_MIN + #if !AVAILABLE_EILINE(X_MIN_PIN) + #error "X_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(X_MIN_PIN); + #endif + #if HAS_Y_MAX + #if !AVAILABLE_EILINE(Y_MAX_PIN) + #error "Y_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(Y_MAX_PIN); + #endif + #if HAS_Y_MIN + #if !AVAILABLE_EILINE(Y_MIN_PIN) + #error "Y_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(Y_MIN_PIN); + #endif + #if HAS_Z_MAX + #if !AVAILABLE_EILINE(Z_MAX_PIN) + #error "Z_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(Z_MAX_PIN); + #endif + #if HAS_Z_MIN + #if !AVAILABLE_EILINE(Z_MIN_PIN) + #error "Z_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(Z_MIN_PIN); + #endif + #if HAS_Z2_MAX + #if !AVAILABLE_EILINE(Z2_MAX_PIN) + #error "Z2_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(Z2_MAX_PIN); + #endif + #if HAS_Z2_MIN + #if !AVAILABLE_EILINE(Z2_MIN_PIN) + #error "Z2_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(Z2_MIN_PIN); + #endif + #if HAS_Z3_MAX + #if !AVAILABLE_EILINE(Z3_MAX_PIN) + #error "Z3_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(Z3_MAX_PIN); + #endif + #if HAS_Z3_MIN + #if !AVAILABLE_EILINE(Z3_MIN_PIN) + #error "Z3_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(Z3_MIN_PIN); + #endif + #if HAS_Z4_MAX + #if !AVAILABLE_EILINE(Z4_MAX_PIN) + #error "Z4_MAX_PIN has no EXTINT line available." + #endif + _ATTACH(Z4_MAX_PIN); + #endif + #if HAS_Z4_MIN + #if !AVAILABLE_EILINE(Z4_MIN_PIN) + #error "Z4_MIN_PIN has no EXTINT line available." + #endif + _ATTACH(Z4_MIN_PIN); + #endif + #if HAS_Z_MIN_PROBE_PIN + #if !AVAILABLE_EILINE(Z_MIN_PROBE_PIN) + #error "Z_MIN_PROBE_PIN has no EXTINT line available." + #endif + _ATTACH(Z_MIN_PROBE_PIN); + #endif + #if HAS_I_MAX + #if !AVAILABLE_EILINE(I_MAX_PIN) + #error "I_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(I_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_I_MIN + #if !AVAILABLE_EILINE(I_MIN_PIN) + #error "I_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(I_MIN_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_J_MAX + #if !AVAILABLE_EILINE(J_MAX_PIN) + #error "J_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(J_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_J_MIN + #if !AVAILABLE_EILINE(J_MIN_PIN) + #error "J_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(J_MIN_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_K_MAX + #if !AVAILABLE_EILINE(K_MAX_PIN) + #error "K_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(K_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_K_MIN + #if !AVAILABLE_EILINE(K_MIN_PIN) + #error "K_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(K_MIN_PIN, endstop_ISR, CHANGE); + #endif +} diff --git a/src/HAL/SAMD51/fastio.h b/src/HAL/SAMD51/fastio.h new file mode 100644 index 0000000..79aede5 --- /dev/null +++ b/src/HAL/SAMD51/fastio.h @@ -0,0 +1,253 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast IO functions for SAMD51 + */ + +#include "SAMD51.h" + +/** + * Utility functions + */ + +#ifndef MASK + #define MASK(PIN) _BV(PIN) +#endif + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(IO); WRITE(IO, HIGH); WRITE(IO, LOW); + */ + +// Read a pin +#define READ(IO) ((PORT->Group[(EPortType)GET_SAMD_PORT(IO)].IN.reg & MASK(GET_SAMD_PIN(IO))) != 0) + +// Write to a pin +#define WRITE(IO,V) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t mask = MASK(GET_SAMD_PIN(IO)); \ + \ + if (V) PORT->Group[port].OUTSET.reg = mask; \ + else PORT->Group[port].OUTCLR.reg = mask; \ + }while(0) + +// Toggle a pin +#define TOGGLE(IO) PORT->Group[(EPortType)GET_SAMD_PORT(IO)].OUTTGL.reg = MASK(GET_SAMD_PIN(IO)); + +// Set pin as input +#define SET_INPUT(IO) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t pin = GET_SAMD_PIN(IO); \ + \ + PORT->Group[port].PINCFG[pin].reg = (uint8_t)(PORT_PINCFG_INEN); \ + PORT->Group[port].DIRCLR.reg = MASK(pin); \ + }while(0) +// Set pin as input with pullup +#define SET_INPUT_PULLUP(IO) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t pin = GET_SAMD_PIN(IO); \ + const uint32_t mask = MASK(pin); \ + \ + PORT->Group[port].PINCFG[pin].reg = (uint8_t)(PORT_PINCFG_INEN | PORT_PINCFG_PULLEN); \ + PORT->Group[port].DIRCLR.reg = mask; \ + PORT->Group[port].OUTSET.reg = mask; \ + }while(0) +// Set pin as input with pulldown +#define SET_INPUT_PULLDOWN(IO) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t pin = GET_SAMD_PIN(IO); \ + const uint32_t mask = MASK(pin); \ + \ + PORT->Group[port].PINCFG[pin].reg = (uint8_t)(PORT_PINCFG_INEN | PORT_PINCFG_PULLEN); \ + PORT->Group[port].DIRCLR.reg = mask; \ + PORT->Group[port].OUTCLR.reg = mask; \ + }while(0) +// Set pin as output (push pull) +#define SET_OUTPUT(IO) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t pin = GET_SAMD_PIN(IO); \ + \ + PORT->Group[port].DIRSET.reg = MASK(pin); \ + PORT->Group[port].PINCFG[pin].reg = 0; \ + }while(0) +// Set pin as output (open drain) +#define SET_OUTPUT_OD(IO) do{ \ + const EPortType port = (EPortType)GET_SAMD_PORT(IO); \ + const uint32_t pin = GET_SAMD_PIN(IO); \ + \ + PORT->Group[port].PINCFG[pin].reg = (uint8_t)(PORT_PINCFG_PULLEN); \ + PORT->Group[port].DIRCLR.reg = MASK(pin); \ + }while(0) +// Set pin as PWM (push pull) +#define SET_PWM SET_OUTPUT +// Set pin as PWM (open drain) +#define SET_PWM_OD SET_OUTPUT_OD + +// check if pin is an output +#define IS_OUTPUT(IO) ((PORT->Group[(EPortType)GET_SAMD_PORT(IO)].DIR.reg & MASK(GET_SAMD_PIN(IO))) \ + || (PORT->Group[(EPortType)GET_SAMD_PORT(IO)].PINCFG[GET_SAMD_PIN(IO)].reg & (PORT_PINCFG_INEN | PORT_PINCFG_PULLEN)) == PORT_PINCFG_PULLEN) +// check if pin is an input +#define IS_INPUT(IO) !IS_OUTPUT(IO) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) +#define OUT_WRITE_OD(IO,V) do{ SET_OUTPUT_OD(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +/** + * Ports and functions + * Added as necessary or if I feel like it- not a comprehensive list! + */ + +#ifdef ADAFRUIT_GRAND_CENTRAL_M4 + + /* + * Adafruit Grand Central M4 has a lot of PWMs the availables are listed here. + * Some of these share the same source and so can't be used in the same time + */ + #define PWM_PIN(P) (WITHIN(P, 2, 13) || WITHIN(P, 22, 23) || WITHIN(P, 44, 45) || P == 48) + + // Return fulfilled ADCx->INPUTCTRL.reg + #define PIN_TO_INPUTCTRL(P) ( (PIN_TO_AIN(P) == 0) ? ADC_INPUTCTRL_MUXPOS_AIN0 \ + : (PIN_TO_AIN(P) == 1) ? ADC_INPUTCTRL_MUXPOS_AIN1 \ + : (PIN_TO_AIN(P) == 2) ? ADC_INPUTCTRL_MUXPOS_AIN2 \ + : (PIN_TO_AIN(P) == 3) ? ADC_INPUTCTRL_MUXPOS_AIN3 \ + : (PIN_TO_AIN(P) == 4) ? ADC_INPUTCTRL_MUXPOS_AIN4 \ + : (PIN_TO_AIN(P) == 5) ? ADC_INPUTCTRL_MUXPOS_AIN5 \ + : (PIN_TO_AIN(P) == 6) ? ADC_INPUTCTRL_MUXPOS_AIN6 \ + : (PIN_TO_AIN(P) == 7) ? ADC_INPUTCTRL_MUXPOS_AIN7 \ + : (PIN_TO_AIN(P) == 8) ? ADC_INPUTCTRL_MUXPOS_AIN8 \ + : (PIN_TO_AIN(P) == 9) ? ADC_INPUTCTRL_MUXPOS_AIN9 \ + : (PIN_TO_AIN(P) == 10) ? ADC_INPUTCTRL_MUXPOS_AIN10 \ + : (PIN_TO_AIN(P) == 11) ? ADC_INPUTCTRL_MUXPOS_AIN11 \ + : (PIN_TO_AIN(P) == 12) ? ADC_INPUTCTRL_MUXPOS_AIN12 \ + : (PIN_TO_AIN(P) == 13) ? ADC_INPUTCTRL_MUXPOS_AIN13 \ + : (PIN_TO_AIN(P) == 14) ? ADC_INPUTCTRL_MUXPOS_AIN14 \ + : ADC_INPUTCTRL_MUXPOS_AIN15) + + #define ANAPIN_TO_SAMDPIN(P) ( (P == 0) ? PIN_TO_SAMD_PIN(67) \ + : (P == 1) ? PIN_TO_SAMD_PIN(68) \ + : (P == 2) ? PIN_TO_SAMD_PIN(69) \ + : (P == 3) ? PIN_TO_SAMD_PIN(70) \ + : (P == 4) ? PIN_TO_SAMD_PIN(71) \ + : (P == 5) ? PIN_TO_SAMD_PIN(72) \ + : (P == 6) ? PIN_TO_SAMD_PIN(73) \ + : (P == 7) ? PIN_TO_SAMD_PIN(74) \ + : (P == 8) ? PIN_TO_SAMD_PIN(54) \ + : (P == 9) ? PIN_TO_SAMD_PIN(55) \ + : (P == 10) ? PIN_TO_SAMD_PIN(56) \ + : (P == 11) ? PIN_TO_SAMD_PIN(57) \ + : (P == 12) ? PIN_TO_SAMD_PIN(58) \ + : (P == 13) ? PIN_TO_SAMD_PIN(59) \ + : (P == 14) ? PIN_TO_SAMD_PIN(60) \ + : (P == 15) ? PIN_TO_SAMD_PIN(61) \ + : (P == 16) ? PIN_TO_SAMD_PIN(12) \ + : (P == 17) ? PIN_TO_SAMD_PIN(13) \ + : PIN_TO_SAMD_PIN(9)) + + #define digitalPinToAnalogInput(P) (WITHIN(P, 67, 74) ? (P) - 67 : WITHIN(P, 54, 61) ? 8 + (P) - 54 : WITHIN(P, 12, 13) ? 16 + (P) - 12 : P == 9 ? 18 : -1) + + /* + * pins + */ + + // PORTA + #define DIO67_PIN PIN_PA02 // A0 + #define DIO59_PIN PIN_PA04 // A13 + #define DIO68_PIN PIN_PA05 // A1 + #define DIO60_PIN PIN_PA06 // A14 + #define DIO61_PIN PIN_PA07 // A15 + #define DIO26_PIN PIN_PA12 + #define DIO27_PIN PIN_PA13 + #define DIO28_PIN PIN_PA14 + #define DIO23_PIN PIN_PA15 + #define DIO37_PIN PIN_PA16 + #define DIO36_PIN PIN_PA17 + #define DIO35_PIN PIN_PA18 + #define DIO34_PIN PIN_PA19 + #define DIO33_PIN PIN_PA20 + #define DIO32_PIN PIN_PA21 + #define DIO31_PIN PIN_PA22 + #define DIO30_PIN PIN_PA23 + // PORTB + #define DIO12_PIN PIN_PB00 // A16 + #define DIO13_PIN PIN_PB01 // A17 + #define DIO9_PIN PIN_PB02 // A18 + #define DIO69_PIN PIN_PB03 // A2 + #define DIO74_PIN PIN_PB04 // A7 + #define DIO54_PIN PIN_PB05 // A8 + #define DIO55_PIN PIN_PB06 // A9 + #define DIO56_PIN PIN_PB07 // A10 + #define DIO57_PIN PIN_PB08 // A11 + #define DIO58_PIN PIN_PB09 // A12 + #define DIO18_PIN PIN_PB12 + #define DIO19_PIN PIN_PB13 + #define DIO39_PIN PIN_PB14 + #define DIO38_PIN PIN_PB15 + #define DIO14_PIN PIN_PB16 + #define DIO15_PIN PIN_PB17 + #define DIO8_PIN PIN_PB18 + #define DIO29_PIN PIN_PB19 + #define DIO20_PIN PIN_PB20 + #define DIO21_PIN PIN_PB21 + #define DIO10_PIN PIN_PB22 + #define DIO11_PIN PIN_PB23 + #define DIO1_PIN PIN_PB24 + #define DIO0_PIN PIN_PB25 + #define DIO83_PIN PIN_PB28 // SD_CS + #define DIO95_PIN PIN_PB31 // SD_CD + // PORTC + #define DIO70_PIN PIN_PC00 // A3 + #define DIO71_PIN PIN_PC01 // A4 + #define DIO72_PIN PIN_PC02 // A5 + #define DIO73_PIN PIN_PC03 // A6 + #define DIO48_PIN PIN_PC04 + #define DIO49_PIN PIN_PC05 + #define DIO46_PIN PIN_PC06 + #define DIO47_PIN PIN_PC07 + #define DIO45_PIN PIN_PC10 + #define DIO44_PIN PIN_PC11 + #define DIO41_PIN PIN_PC12 + #define DIO40_PIN PIN_PC13 + #define DIO43_PIN PIN_PC14 + #define DIO42_PIN PIN_PC15 + #define DIO25_PIN PIN_PC16 + #define DIO24_PIN PIN_PC17 + #define DIO2_PIN PIN_PC18 + #define DIO3_PIN PIN_PC19 + #define DIO4_PIN PIN_PC20 + #define DIO5_PIN PIN_PC21 + #define DIO16_PIN PIN_PC22 + #define DIO17_PIN PIN_PC23 + #define DIO88_PIN PIN_PC24 // NEOPIXEL + // PORTD + #define DIO53_PIN PIN_PD10 + #define DIO22_PIN PIN_PD12 + #define DIO6_PIN PIN_PD20 + #define DIO7_PIN PIN_PD21 + +#endif // ADAFRUIT_GRAND_CENTRAL_M4 diff --git a/src/HAL/SAMD51/inc/Conditionals_LCD.h b/src/HAL/SAMD51/inc/Conditionals_LCD.h new file mode 100644 index 0000000..932348c --- /dev/null +++ b/src/HAL/SAMD51/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/SAMD51." +#endif diff --git a/src/HAL/SAMD51/inc/Conditionals_adv.h b/src/HAL/SAMD51/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/SAMD51/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/SAMD51/inc/Conditionals_post.h b/src/HAL/SAMD51/inc/Conditionals_post.h new file mode 100644 index 0000000..ce6d3fd --- /dev/null +++ b/src/HAL/SAMD51/inc/Conditionals_post.h @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define FLASH_EEPROM_EMULATION +#elif EITHER(I2C_EEPROM, SPI_EEPROM) + #define USE_SHARED_EEPROM 1 +#endif diff --git a/src/HAL/SAMD51/inc/SanityCheck.h b/src/HAL/SAMD51/inc/SanityCheck.h new file mode 100644 index 0000000..1b876c9 --- /dev/null +++ b/src/HAL/SAMD51/inc/SanityCheck.h @@ -0,0 +1,57 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Test SAMD51 specific configuration values for errors at compile-time. + */ + +#if ENABLED(FLASH_EEPROM_EMULATION) + #warning "Did you activate the SmartEEPROM? See https://github.com/GMagician/SAMD51-SmartEEprom-Manager/releases" +#endif + +#if defined(ADAFRUIT_GRAND_CENTRAL_M4) && SD_CONNECTION_IS(CUSTOM_CABLE) + #error "No custom SD drive cable defined for this board." +#endif + +#if (defined(TEMP_0_SCK_PIN) && defined(TEMP_0_MISO_PIN) && (TEMP_0_SCK_PIN == SCK1 || TEMP_0_MISO_PIN == MISO1)) || \ + (defined(TEMP_1_SCK_PIN) && defined(TEMP_1_MISO_PIN) && (TEMP_1_SCK_PIN == SCK1 || TEMP_1_MISO_PIN == MISO1)) + #error "OnBoard SPI BUS can't be shared with other devices." +#endif + +#if SERVO_TC == MF_TIMER_RTC + #error "Servos can't use RTC timer" +#endif + +#if ENABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is not yet implemented for SAMD51. Disable EMERGENCY_PARSER to continue." +#endif + +#if ENABLED(SDIO_SUPPORT) + #error "SDIO_SUPPORT is not supported on SAMD51." +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on SAMD51." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on AGCM4." +#endif diff --git a/src/HAL/SAMD51/pinsDebug.h b/src/HAL/SAMD51/pinsDebug.h new file mode 100644 index 0000000..f0a46fd --- /dev/null +++ b/src/HAL/SAMD51/pinsDebug.h @@ -0,0 +1,154 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define NUMBER_PINS_TOTAL PINS_COUNT + +#define digitalRead_mod(p) extDigitalRead(p) +#define PRINT_PORT(p) do{ SERIAL_ECHOPGM(" Port: "); sprintf_P(buffer, PSTR("%c%02ld"), 'A' + g_APinDescription[p].ulPort, g_APinDescription[p].ulPin); SERIAL_ECHO(buffer); }while (0) +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital +#define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL) +#define DIGITAL_PIN_TO_ANALOG_PIN(p) digitalPinToAnalogInput(p) +#define IS_ANALOG(P) (DIGITAL_PIN_TO_ANALOG_PIN(P)!=-1) +#define pwm_status(pin) digitalPinHasPWM(pin) +#define MULTI_NAME_PAD 27 // space needed to be pretty if not first name assigned to a pin + +// pins that will cause hang/reset/disconnect in M43 Toggle and Watch utilities +// uses pin index +#define M43_NEVER_TOUCH(Q) ((Q) >= 75) + +bool GET_PINMODE(int8_t pin) { // 1: output, 0: input + const EPortType samdport = g_APinDescription[pin].ulPort; + const uint32_t samdpin = g_APinDescription[pin].ulPin; + return PORT->Group[samdport].DIR.reg & MASK(samdpin) || (PORT->Group[samdport].PINCFG[samdpin].reg & (PORT_PINCFG_INEN | PORT_PINCFG_PULLEN)) == PORT_PINCFG_PULLEN; +} + +void pwm_details(int32_t pin) { + if (pwm_status(pin)) { + //uint32_t chan = g_APinDescription[pin].ulPWMChannel TODO when fast pwm is operative; + //SERIAL_ECHOPGM("PWM = ", duty); + } +} + +/** + * AGCM4 Board pin | PORT | Label + * ----------------+--------+------- + * 0 | PB25 | "RX0" + * 1 | PB24 | "TX0" + * 2 | PC18 | + * 3 | PC19 | + * 4 | PC20 | + * 5 | PC21 | + * 6 | PD20 | + * 7 | PD21 | + * 8 | PB18 | + * 9 | PB2 | + * 10 | PB22 | + * 11 | PB23 | + * 12 | PB0 | "A16" + * 13 | PB1 | LED AMBER "L" / "A17" + * 14 | PB16 | "TX3" + * 15 | PB17 | "RX3" + * 16 | PC22 | "TX2" + * 17 | PC23 | "RX2" + * 18 | PB12 | "TX1" / "A18" + * 19 | PB13 | "RX1" + * 20 | PB20 | "SDA" + * 21 | PB21 | "SCL" + * 22 | PD12 | + * 23 | PA15 | + * 24 | PC17 | + * 25 | PC16 | + * 26 | PA12 | + * 27 | PA13 | + * 28 | PA14 | + * 29 | PB19 | + * 30 | PA23 | + * 31 | PA22 | + * 32 | PA21 | + * 33 | PA20 | + * 34 | PA19 | + * 35 | PA18 | + * 36 | PA17 | + * 37 | PA16 | + * 38 | PB15 | + * 39 | PB14 | + * 40 | PC13 | + * 41 | PC12 | + * 42 | PC15 | + * 43 | PC14 | + * 44 | PC11 | + * 45 | PC10 | + * 46 | PC6 | + * 47 | PC7 | + * 48 | PC4 | + * 49 | PC5 | + * 50 | PD11 | + * 51 | PD8 | + * 52 | PD9 | + * 53 | PD10 | + * 54 | PB5 | "A8" + * 55 | PB6 | "A9" + * 56 | PB7 | "A10" + * 57 | PB8 | "A11" + * 58 | PB9 | "A12" + * 69 | PA4 | "A13" + * 60 | PA6 | "A14" + * 61 | PA7 | "A15" + * 62 | PB17 | + * 63 | PB20 | + * 64 | PD11 | + * 65 | PD8 | + * 66 | PD9 | + * 67 | PA2 | "A0" / "DAC0" + * 68 | PA5 | "A1" / "DAC1" + * 69 | PB3 | "A2" + * 70 | PC0 | "A3" + * 71 | PC1 | "A4" + * 72 | PC2 | "A5" + * 73 | PC3 | "A6" + * 74 | PB4 | "A7" + * 75 | PC31 | LED GREEN "RX" + * 76 | PC30 | LED GREEN "TX" + * 77 | PA27 | USB: Host enable + * 78 | PA24 | USB: D- + * 79 | PA25 | USB: D+ + * 80 | PB29 | SD: MISO + * 81 | PB27 | SD: SCK + * 82 | PB26 | SD: MOSI + * 83 | PB28 | SD: CS + * 84 | PA3 | AREF + * 85 | PA2 | DAC0 (Duplicate) + * 86 | PA5 | DAC1 (Duplicate) + * 87 | PB1 | LED AMBER "L" (Duplicate) + * 88 | PC24 | NeoPixel + * 89 | PB10 | QSPI: SCK + * 90 | PB11 | QSPI: CS + * 91 | PA8 | QSPI: IO0 + * 92 | PA9 | QSPI: IO1 + * 93 | PA10 | QSPI: IO2 + * 94 | PA11 | QSPI: IO3 + * 95 | PB31 | SD: DETECT + */ diff --git a/src/HAL/SAMD51/spi_pins.h b/src/HAL/SAMD51/spi_pins.h new file mode 100644 index 0000000..2a667bc --- /dev/null +++ b/src/HAL/SAMD51/spi_pins.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifdef ADAFRUIT_GRAND_CENTRAL_M4 + + /* + * AGCM4 Default SPI Pins + * + * SS SCK MISO MOSI + * +-------------------------+ + * SPI | 53 52 50 51 | + * SPI1 | 83 81 80 82 | + * +-------------------------+ + * Any pin can be used for Chip Select (SD_SS_PIN) + */ + #ifndef SD_SCK_PIN + #define SD_SCK_PIN 52 + #endif + #ifndef SD_MISO_PIN + #define SD_MISO_PIN 50 + #endif + #ifndef SD_MOSI_PIN + #define SD_MOSI_PIN 51 + #endif + #ifndef SDSS + #define SDSS 53 + #endif + +#else + + #error "Unsupported board!" + +#endif + +#define SD_SS_PIN SDSS diff --git a/src/HAL/SAMD51/timers.cpp b/src/HAL/SAMD51/timers.cpp new file mode 100644 index 0000000..1ad0e36 --- /dev/null +++ b/src/HAL/SAMD51/timers.cpp @@ -0,0 +1,168 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __SAMD51__ + +// -------------------------------------------------------------------------- +// Includes +// -------------------------------------------------------------------------- + +#include "../../inc/MarlinConfig.h" +#include "ServoTimers.h" // for SERVO_TC + +// -------------------------------------------------------------------------- +// Local defines +// -------------------------------------------------------------------------- + +#define NUM_HARDWARE_TIMERS 9 + +// -------------------------------------------------------------------------- +// Private Variables +// -------------------------------------------------------------------------- + +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { + { {.pTc=TC0}, TC0_IRQn, TC_PRIORITY(0) }, // 0 - stepper (assigned priority 2) + { {.pTc=TC1}, TC1_IRQn, TC_PRIORITY(1) }, // 1 - stepper (needed by 32 bit timers) + { {.pTc=TC2}, TC2_IRQn, 5 }, // 2 - tone (reserved by framework and fixed assigned priority 5) + { {.pTc=TC3}, TC3_IRQn, TC_PRIORITY(3) }, // 3 - servo (assigned priority 1) + { {.pTc=TC4}, TC4_IRQn, TC_PRIORITY(4) }, // 4 - software serial (no interrupts used) + { {.pTc=TC5}, TC5_IRQn, TC_PRIORITY(5) }, + { {.pTc=TC6}, TC6_IRQn, TC_PRIORITY(6) }, + { {.pTc=TC7}, TC7_IRQn, TC_PRIORITY(7) }, + { {.pRtc=RTC}, RTC_IRQn, TC_PRIORITY(8) } // 8 - temperature (assigned priority 6) +}; + +// -------------------------------------------------------------------------- +// Private functions +// -------------------------------------------------------------------------- + +FORCE_INLINE void Disable_Irq(IRQn_Type irq) { + NVIC_DisableIRQ(irq); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); +} + +// -------------------------------------------------------------------------- +// Public functions +// -------------------------------------------------------------------------- + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + + // Disable interrupt, just in case it was already enabled + Disable_Irq(irq); + + if (timer_num == MF_TIMER_RTC) { + Rtc * const rtc = timer_config[timer_num].pRtc; + + // Disable timer interrupt + rtc->MODE0.INTENCLR.reg = RTC_MODE0_INTENCLR_CMP0; + + // RTC clock setup + OSC32KCTRL->RTCCTRL.reg = OSC32KCTRL_RTCCTRL_RTCSEL_XOSC32K; // External 32.768kHz oscillator + + // Stop timer, just in case, to be able to reconfigure it + rtc->MODE0.CTRLA.bit.ENABLE = false; + SYNC(rtc->MODE0.SYNCBUSY.bit.ENABLE); + + // Mode, reset counter on match + rtc->MODE0.CTRLA.reg = RTC_MODE0_CTRLA_MODE_COUNT32 | RTC_MODE0_CTRLA_MATCHCLR; + + // Set compare value + rtc->MODE0.COMP[0].reg = (32768 + frequency / 2) / frequency; + SYNC(rtc->MODE0.SYNCBUSY.bit.COMP0); + + // Enable interrupt on compare + rtc->MODE0.INTFLAG.reg = RTC_MODE0_INTFLAG_CMP0; // reset pending interrupt + rtc->MODE0.INTENSET.reg = RTC_MODE0_INTENSET_CMP0; // enable compare 0 interrupt + + // And start timer + rtc->MODE0.CTRLA.bit.ENABLE = true; + SYNC(rtc->MODE0.SYNCBUSY.bit.ENABLE); + } + else { + Tc * const tc = timer_config[timer_num].pTc; + + // Disable timer interrupt + tc->COUNT32.INTENCLR.reg = TC_INTENCLR_OVF; // disable overflow interrupt + + // TCn clock setup + const uint8_t clockID = GCLK_CLKCTRL_IDs[TCC_INST_NUM + timer_num]; // TC clock are preceded by TCC ones + GCLK->PCHCTRL[clockID].bit.CHEN = false; + SYNC(GCLK->PCHCTRL[clockID].bit.CHEN); + GCLK->PCHCTRL[clockID].reg = GCLK_PCHCTRL_GEN_GCLK0 | GCLK_PCHCTRL_CHEN; // 120MHz startup code programmed + SYNC(!GCLK->PCHCTRL[clockID].bit.CHEN); + + // Stop timer, just in case, to be able to reconfigure it + tc->COUNT32.CTRLA.bit.ENABLE = false; + SYNC(tc->COUNT32.SYNCBUSY.bit.ENABLE); + + // Reset timer + tc->COUNT32.CTRLA.bit.SWRST = true; + SYNC(tc->COUNT32.SYNCBUSY.bit.SWRST); + + // Wave mode, reset counter on compare match + tc->COUNT32.WAVE.reg = TC_WAVE_WAVEGEN_MFRQ; + tc->COUNT32.CTRLA.reg = TC_CTRLA_MODE_COUNT32 | TC_CTRLA_PRESCALER_DIV1; + tc->COUNT32.CTRLBCLR.reg = TC_CTRLBCLR_DIR; + SYNC(tc->COUNT32.SYNCBUSY.bit.CTRLB); + + // Set compare value + tc->COUNT32.CC[0].reg = (HAL_TIMER_RATE) / frequency; + tc->COUNT32.COUNT.reg = 0; + + // Enable interrupt on compare + tc->COUNT32.INTFLAG.reg = TC_INTFLAG_OVF; // reset pending interrupt + tc->COUNT32.INTENSET.reg = TC_INTENSET_OVF; // enable overflow interrupt + + // And start timer + tc->COUNT32.CTRLA.bit.ENABLE = true; + SYNC(tc->COUNT32.SYNCBUSY.bit.ENABLE); + } + + // Finally, enable IRQ + NVIC_SetPriority(irq, timer_config[timer_num].priority); + NVIC_EnableIRQ(irq); +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; + NVIC_EnableIRQ(irq); +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; + Disable_Irq(irq); +} + +// missing from CMSIS: Check if interrupt is enabled or not +static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) { + return TEST(NVIC->ISER[uint32_t(IRQn) >> 5], uint32_t(IRQn) & 0x1F); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; + return NVIC_GetEnabledIRQ(irq); +} + +#endif // __SAMD51__ diff --git a/src/HAL/SAMD51/timers.h b/src/HAL/SAMD51/timers.h new file mode 100644 index 0000000..86e980c --- /dev/null +++ b/src/HAL/SAMD51/timers.h @@ -0,0 +1,143 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +// -------------------------------------------------------------------------- +// Defines +// -------------------------------------------------------------------------- + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define HAL_TIMER_RATE F_CPU // frequency of timers peripherals + +#define MF_TIMER_RTC 8 // This is not a TC but a RTC + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP MF_TIMER_RTC // Timer Index for Temperature +#endif + +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US (STEPPER_TIMER_RATE / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#define TC_PRIORITY(t) ( t == SERVO_TC ? 1 \ + : (t == MF_TIMER_STEP || t == MF_TIMER_PULSE) ? 2 \ + : (t == MF_TIMER_TEMP) ? 6 : 7 ) + +#define _TC_HANDLER(t) void TC##t##_Handler() +#define TC_HANDLER(t) _TC_HANDLER(t) +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() TC_HANDLER(MF_TIMER_STEP) +#endif +#if MF_TIMER_STEP != MF_TIMER_PULSE + #define HAL_PULSE_TIMER_ISR() TC_HANDLER(MF_TIMER_PULSE) +#endif +#if MF_TIMER_TEMP == MF_TIMER_RTC + #define HAL_TEMP_TIMER_ISR() void RTC_Handler() +#else + #define HAL_TEMP_TIMER_ISR() TC_HANDLER(MF_TIMER_TEMP) +#endif + +// -------------------------------------------------------------------------- +// Types +// -------------------------------------------------------------------------- + +typedef struct { + union { + Tc *pTc; + Rtc *pRtc; + }; + IRQn_Type IRQ_Id; + uint8_t priority; +} tTimerConfig; + +// -------------------------------------------------------------------------- +// Public Variables +// -------------------------------------------------------------------------- + +extern const tTimerConfig timer_config[]; + +// -------------------------------------------------------------------------- +// Public functions +// -------------------------------------------------------------------------- + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; + tc->COUNT32.CC[0].reg = compare; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; + return (hal_timer_t)tc->COUNT32.CC[0].reg; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; + tc->COUNT32.CTRLBSET.reg = TC_CTRLBCLR_CMD_READSYNC; + SYNC(tc->COUNT32.SYNCBUSY.bit.CTRLB || tc->COUNT32.SYNCBUSY.bit.COUNT); + return tc->COUNT32.COUNT.reg; +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { + if (timer_num == MF_TIMER_RTC) { + Rtc * const rtc = timer_config[timer_num].pRtc; + // Clear interrupt flag + rtc->MODE0.INTFLAG.reg = RTC_MODE0_INTFLAG_CMP0; + } + else { + Tc * const tc = timer_config[timer_num].pTc; + // Clear interrupt flag + tc->COUNT32.INTFLAG.reg = TC_INTFLAG_OVF; + } +} + +#define HAL_timer_isr_epilogue(timer_num) diff --git a/src/HAL/STM32/HAL.cpp b/src/HAL/STM32/HAL.cpp new file mode 100644 index 0000000..aff52f5 --- /dev/null +++ b/src/HAL/STM32/HAL.cpp @@ -0,0 +1,182 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" +#include "../shared/Delay.h" + +#include "usb_serial.h" + +#ifdef USBCON + DefaultSerial1 MSerialUSB(false, SerialUSB); +#endif + +#if ENABLED(SRAM_EEPROM_EMULATION) + #if STM32F7xx + #include + #elif STM32F4xx + #include + #else + #error "SRAM_EEPROM_EMULATION is currently only supported for STM32F4xx and STM32F7xx" + #endif +#endif + +#if HAS_SD_HOST_DRIVE + #include "msc_sd.h" + #include "usbd_cdc_if.h" +#endif + +// ------------------------ +// Public Variables +// ------------------------ + +uint16_t MarlinHAL::adc_result; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(POSTMORTEM_DEBUGGING) + extern void install_min_serial(); +#endif + +// HAL initialization task +void MarlinHAL::init() { + // Ensure F_CPU is a constant expression. + // If the compiler breaks here, it means that delay code that should compute at compile time will not work. + // So better safe than sorry here. + constexpr int cpuFreq = F_CPU; + UNUSED(cpuFreq); + + #if ENABLED(SDSUPPORT) && DISABLED(SDIO_SUPPORT) && (defined(SDSS) && SDSS != -1) + OUT_WRITE(SDSS, HIGH); // Try to set SDSS inactive before any other SPI users start up + #endif + + #if PIN_EXISTS(LED) + OUT_WRITE(LED_PIN, LOW); + #endif + + #if ENABLED(SRAM_EEPROM_EMULATION) + __HAL_RCC_PWR_CLK_ENABLE(); + HAL_PWR_EnableBkUpAccess(); // Enable access to backup SRAM + __HAL_RCC_BKPSRAM_CLK_ENABLE(); + LL_PWR_EnableBkUpRegulator(); // Enable backup regulator + while (!LL_PWR_IsActiveFlag_BRR()); // Wait until backup regulator is initialized + #endif + + SetTimerInterruptPriorities(); + + #if ENABLED(EMERGENCY_PARSER) && (USBD_USE_CDC || USBD_USE_CDC_MSC) + USB_Hook_init(); + #endif + + TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler + + TERN_(HAS_SD_HOST_DRIVE, MSC_SD_init()); // Enable USB SD card access + + #if PIN_EXISTS(USB_CONNECT) + OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING); // USB clear connection + delay(1000); // Give OS time to notice + WRITE(USB_CONNECT_PIN, USB_CONNECT_INVERTING); + #endif +} + +// HAL idle task +void MarlinHAL::idletask() { + #if HAS_SHARED_MEDIA + // Stm32duino currently doesn't have a "loop/idle" method + CDC_resume_receive(); + CDC_continue_transmit(); + #endif +} + +void MarlinHAL::reboot() { NVIC_SystemReset(); } + +uint8_t MarlinHAL::get_reset_source() { + return + #ifdef RCC_FLAG_IWDGRST // Some sources may not exist... + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) ? RST_WATCHDOG : + #endif + #ifdef RCC_FLAG_IWDG1RST + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST) ? RST_WATCHDOG : + #endif + #ifdef RCC_FLAG_IWDG2RST + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_IWDG2RST) ? RST_WATCHDOG : + #endif + #ifdef RCC_FLAG_SFTRST + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST) ? RST_SOFTWARE : + #endif + #ifdef RCC_FLAG_PINRST + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_PINRST) ? RST_EXTERNAL : + #endif + #ifdef RCC_FLAG_PORRST + RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_PORRST) ? RST_POWER_ON : + #endif + 0 + ; +} + +void MarlinHAL::clear_reset_source() { __HAL_RCC_CLEAR_RESET_FLAGS(); } + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout + + #include + + void MarlinHAL::watchdog_init() { + IF_DISABLED(DISABLE_WATCHDOG_INIT, IWatchdog.begin(WDT_TIMEOUT_US)); + } + + void MarlinHAL::watchdog_refresh() { + IWatchdog.reload(); + #if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED) + TOGGLE(LED_PIN); // heartbeat indicator + #endif + } + +#endif + +extern "C" { + extern unsigned int _ebss; // end of bss section +} + +// Reset the system to initiate a firmware flash +WEAK void flashFirmware(const int16_t) { hal.reboot(); } + +// Maple Compatibility +volatile uint32_t systick_uptime_millis = 0; +systickCallback_t systick_user_callback; +void systick_attach_callback(systickCallback_t cb) { systick_user_callback = cb; } +void HAL_SYSTICK_Callback() { + systick_uptime_millis++; + if (systick_user_callback) systick_user_callback(); +} + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/HAL.h b/src/HAL/STM32/HAL.h new file mode 100644 index 0000000..3e85aca --- /dev/null +++ b/src/HAL/STM32/HAL.h @@ -0,0 +1,281 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define CPU_32_BIT + +#include "../../core/macros.h" +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "Servo.h" +#include "MarlinSerial.h" + +#include "../../inc/MarlinConfigPre.h" + +#include + +// +// Default graphical display delays +// +#define CPU_ST7920_DELAY_1 300 +#define CPU_ST7920_DELAY_2 40 +#define CPU_ST7920_DELAY_3 340 + +// ------------------------ +// Serial ports +// ------------------------ +#ifdef USBCON + #include + #include "../../core/serial_hook.h" + typedef ForwardSerial1Class< decltype(SerialUSB) > DefaultSerial1; + extern DefaultSerial1 MSerialUSB; +#endif + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if WITHIN(SERIAL_PORT, 1, 6) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#elif !defined(USBCON) + #error "SERIAL_PORT must be from 1 to 6." +#elif SERIAL_PORT == -1 + #define MYSERIAL1 MSerialUSB +#else + #error "SERIAL_PORT must be from 1 to 6, or -1 for Native USB." +#endif + +#ifdef SERIAL_PORT_2 + #if WITHIN(SERIAL_PORT_2, 1, 6) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #elif !defined(USBCON) + #error "SERIAL_PORT must be from 1 to 6." + #elif SERIAL_PORT_2 == -1 + #define MYSERIAL2 MSerialUSB + #else + #error "SERIAL_PORT_2 must be from 1 to 6, or -1 for Native USB." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if WITHIN(SERIAL_PORT_3, 1, 6) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #elif !defined(USBCON) + #error "SERIAL_PORT must be from 1 to 6." + #elif SERIAL_PORT_3 == -1 + #define MYSERIAL3 MSerialUSB + #else + #error "SERIAL_PORT_3 must be from 1 to 6, or -1 for Native USB." + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if WITHIN(MMU2_SERIAL_PORT, 1, 6) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #elif !defined(USBCON) + #error "SERIAL_PORT must be from 1 to 6." + #elif MMU2_SERIAL_PORT == -1 + #define MMU2_SERIAL MSerialUSB + #else + #error "MMU2_SERIAL_PORT must be from 1 to 6, or -1 for Native USB." + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if WITHIN(LCD_SERIAL_PORT, 1, 6) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #elif !defined(USBCON) + #error "SERIAL_PORT must be from 1 to 6." + #elif LCD_SERIAL_PORT == -1 + #define LCD_SERIAL MSerialUSB + #else + #error "LCD_SERIAL_PORT must be from 1 to 6, or -1 for Native USB." + #endif + #if HAS_DGUS_LCD + #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite() + #endif +#endif + +/** + * TODO: review this to return 1 for pins that are not analog input + */ +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) (p) +#endif + +// +// Interrupts +// +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() +#define cli() __disable_irq() +#define sei() __enable_irq() + +// ------------------------ +// Types +// ------------------------ + +typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs. + +#ifdef STM32G0B1xx + typedef int32_t pin_t; +#else + typedef int16_t pin_t; +#endif + +class libServo; +typedef libServo hal_servo_t; +#define PAUSE_SERVO_OUTPUT() libServo::pause_all_servos() +#define RESUME_SERVO_OUTPUT() libServo::resume_all_servos() + +// ------------------------ +// ADC +// ------------------------ + +#ifdef ADC_RESOLUTION + #define HAL_ADC_RESOLUTION ADC_RESOLUTION +#else + #define HAL_ADC_RESOLUTION 12 +#endif + +#define HAL_ADC_VREF 3.3 + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +#ifdef STM32F1xx + #define JTAG_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_JTAGDISABLE) + #define JTAGSWD_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_DISABLE) + #define JTAGSWD_RESET() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_RESET); // Reset: FULL SWD+JTAG +#endif + +#define PLATFORM_M997_SUPPORT +void flashFirmware(const int16_t); + +// Maple Compatibility +typedef void (*systickCallback_t)(void); +void systick_attach_callback(systickCallback_t cb); +void HAL_SYSTICK_Callback(); + +extern volatile uint32_t systick_uptime_millis; + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment + +// ------------------------ +// Class Utilities +// ------------------------ + +// Memory related +#define __bss_end __bss_end__ + +extern "C" char* _sbrk(int incr); + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +static inline int freeMemory() { + volatile char top; + return &top - reinterpret_cast(_sbrk(0)); +} + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init() { + analogReadResolution(HAL_ADC_RESOLUTION); + } + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { pinMode(pin, INPUT); } + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin) { adc_result = analogRead(pin); } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the maximum size of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Set the frequency of the timer for the given pin. + * All Timer PWM pins run at the same frequency. + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); + +}; diff --git a/src/HAL/STM32/HAL_SPI.cpp b/src/HAL/STM32/HAL_SPI.cpp new file mode 100644 index 0000000..40d320d --- /dev/null +++ b/src/HAL/STM32/HAL_SPI.cpp @@ -0,0 +1,231 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#include + +// ------------------------ +// Public Variables +// ------------------------ + +static SPISettings spiConfig; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(SOFTWARE_SPI) + + // ------------------------ + // Software SPI + // ------------------------ + + #include "../shared/Delay.h" + + void spiBegin(void) { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + OUT_WRITE(SD_SCK_PIN, HIGH); + SET_INPUT(SD_MISO_PIN); + OUT_WRITE(SD_MOSI_PIN, HIGH); + } + + // Use function with compile-time value so we can actually reach the desired frequency + // Need to adjust this a little bit: on a 72MHz clock, we have 14ns/clock + // and we'll use ~3 cycles to jump to the method and going back, so it'll take ~40ns from the given clock here + #define CALLING_COST_NS (3U * 1000000000U) / (F_CPU) + void (*delaySPIFunc)(); + void delaySPI_125() { DELAY_NS(125 - CALLING_COST_NS); } + void delaySPI_250() { DELAY_NS(250 - CALLING_COST_NS); } + void delaySPI_500() { DELAY_NS(500 - CALLING_COST_NS); } + void delaySPI_1000() { DELAY_NS(1000 - CALLING_COST_NS); } + void delaySPI_2000() { DELAY_NS(2000 - CALLING_COST_NS); } + void delaySPI_4000() { DELAY_NS(4000 - CALLING_COST_NS); } + + void spiInit(uint8_t spiRate) { + // Use datarates Marlin uses + switch (spiRate) { + case SPI_FULL_SPEED: delaySPIFunc = &delaySPI_125; break; // desired: 8,000,000 actual: ~1.1M + case SPI_HALF_SPEED: delaySPIFunc = &delaySPI_125; break; // desired: 4,000,000 actual: ~1.1M + case SPI_QUARTER_SPEED:delaySPIFunc = &delaySPI_250; break; // desired: 2,000,000 actual: ~890K + case SPI_EIGHTH_SPEED: delaySPIFunc = &delaySPI_500; break; // desired: 1,000,000 actual: ~590K + case SPI_SPEED_5: delaySPIFunc = &delaySPI_1000; break; // desired: 500,000 actual: ~360K + case SPI_SPEED_6: delaySPIFunc = &delaySPI_2000; break; // desired: 250,000 actual: ~210K + default: delaySPIFunc = &delaySPI_4000; break; // desired: 125,000 actual: ~123K + } + SPI.begin(); + } + + // Begin SPI transaction, set clock, bit order, data mode + void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { /* do nothing */ } + + uint8_t HAL_SPI_STM32_SpiTransfer_Mode_3(uint8_t b) { // using Mode 3 + for (uint8_t bits = 8; bits--;) { + WRITE(SD_SCK_PIN, LOW); + WRITE(SD_MOSI_PIN, b & 0x80); + + delaySPIFunc(); + WRITE(SD_SCK_PIN, HIGH); + delaySPIFunc(); + + b <<= 1; // little setup time + b |= (READ(SD_MISO_PIN) != 0); + } + DELAY_NS(125); + return b; + } + + // Soft SPI receive byte + uint8_t spiRec() { + hal.isr_off(); // No interrupts during byte receive + const uint8_t data = HAL_SPI_STM32_SpiTransfer_Mode_3(0xFF); + hal.isr_on(); // Enable interrupts + return data; + } + + // Soft SPI read data + void spiRead(uint8_t *buf, uint16_t nbyte) { + for (uint16_t i = 0; i < nbyte; i++) + buf[i] = spiRec(); + } + + // Soft SPI send byte + void spiSend(uint8_t data) { + hal.isr_off(); // No interrupts during byte send + HAL_SPI_STM32_SpiTransfer_Mode_3(data); // Don't care what is received + hal.isr_on(); // Enable interrupts + } + + // Soft SPI send block + void spiSendBlock(uint8_t token, const uint8_t *buf) { + spiSend(token); + for (uint16_t i = 0; i < 512; i++) + spiSend(buf[i]); + } + +#else + + // ------------------------ + // Hardware SPI + // ------------------------ + + /** + * VGPV SPI speed start and PCLK2/2, by default 108/2 = 54Mhz + */ + + /** + * @brief Begin SPI port setup + * + * @return Nothing + * + * @details Only configures SS pin since stm32duino creates and initialize the SPI object + */ + void spiBegin() { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + } + + // Configure SPI for specified SPI speed + void spiInit(uint8_t spiRate) { + // Use datarates Marlin uses + uint32_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = 20000000; break; // 13.9mhz=20000000 6.75mhz=10000000 3.38mhz=5000000 .833mhz=1000000 + case SPI_HALF_SPEED: clock = 5000000; break; + case SPI_QUARTER_SPEED: clock = 2500000; break; + case SPI_EIGHTH_SPEED: clock = 1250000; break; + case SPI_SPEED_5: clock = 625000; break; + case SPI_SPEED_6: clock = 300000; break; + default: + clock = 4000000; // Default from the SPI library + } + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + + SPI.setMISO(SD_MISO_PIN); + SPI.setMOSI(SD_MOSI_PIN); + SPI.setSCLK(SD_SCK_PIN); + + SPI.begin(); + } + + /** + * @brief Receives a single byte from the SPI port. + * + * @return Byte received + * + * @details + */ + uint8_t spiRec() { + uint8_t returnByte = SPI.transfer(0xFF); + return returnByte; + } + + /** + * @brief Receive a number of bytes from the SPI port to a buffer + * + * @param buf Pointer to starting address of buffer to write to. + * @param nbyte Number of bytes to receive. + * @return Nothing + * + * @details Uses DMA + */ + void spiRead(uint8_t *buf, uint16_t nbyte) { + if (nbyte == 0) return; + memset(buf, 0xFF, nbyte); + SPI.transfer(buf, nbyte); + } + + /** + * @brief Send a single byte on SPI port + * + * @param b Byte to send + * + * @details + */ + void spiSend(uint8_t b) { + SPI.transfer(b); + } + + /** + * @brief Write token and then write from 512 byte buffer to SPI (for SD card) + * + * @param buf Pointer with buffer start address + * @return Nothing + * + * @details Use DMA + */ + void spiSendBlock(uint8_t token, const uint8_t *buf) { + uint8_t rxBuf[512]; + SPI.transfer(token); + SPI.transfer((uint8_t*)buf, &rxBuf, 512); + } + +#endif // SOFTWARE_SPI + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/MarlinSPI.cpp b/src/HAL/STM32/MarlinSPI.cpp new file mode 100644 index 0000000..f7c603d --- /dev/null +++ b/src/HAL/STM32/MarlinSPI.cpp @@ -0,0 +1,174 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#if defined(HAL_STM32) && !defined(STM32H7xx) + +#include "MarlinSPI.h" + +static void spi_init(spi_t *obj, uint32_t speed, spi_mode_e mode, uint8_t msb, uint32_t dataSize) { + spi_init(obj, speed, mode, msb); + // spi_init set 8bit always + // TODO: copy the code from spi_init and handle data size, to avoid double init always!! + if (dataSize != SPI_DATASIZE_8BIT) { + obj->handle.Init.DataSize = dataSize; + HAL_SPI_Init(&obj->handle); + __HAL_SPI_ENABLE(&obj->handle); + } +} + +void MarlinSPI::setClockDivider(uint8_t _div) { + _speed = spi_getClkFreq(&_spi);// / _div; + _clockDivider = _div; +} + +void MarlinSPI::begin(void) { + //TODO: only call spi_init if any parameter changed!! + spi_init(&_spi, _speed, _dataMode, _bitOrder, _dataSize); +} + +void MarlinSPI::setupDma(SPI_HandleTypeDef &_spiHandle, DMA_HandleTypeDef &_dmaHandle, uint32_t direction, bool minc) { + _dmaHandle.Init.Direction = direction; + _dmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; + _dmaHandle.Init.Mode = DMA_NORMAL; + _dmaHandle.Init.Priority = DMA_PRIORITY_LOW; + _dmaHandle.Init.MemInc = minc ? DMA_MINC_ENABLE : DMA_MINC_DISABLE; + + if (_dataSize == DATA_SIZE_8BIT) { + _dmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + _dmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + } + else { + _dmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + _dmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + } + #ifdef STM32F4xx + _dmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + #endif + + // start DMA hardware + // TODO: check if hardware is already enabled + #ifdef SPI1_BASE + if (_spiHandle.Instance == SPI1) { + #ifdef STM32F1xx + __HAL_RCC_DMA1_CLK_ENABLE(); + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA1_Channel3 : DMA1_Channel2; + #elif defined(STM32F4xx) + __HAL_RCC_DMA2_CLK_ENABLE(); + _dmaHandle.Init.Channel = DMA_CHANNEL_3; + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA2_Stream3 : DMA2_Stream0; + #endif + } + #endif + #ifdef SPI2_BASE + if (_spiHandle.Instance == SPI2) { + #ifdef STM32F1xx + __HAL_RCC_DMA1_CLK_ENABLE(); + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA1_Channel5 : DMA1_Channel4; + #elif defined(STM32F4xx) + __HAL_RCC_DMA1_CLK_ENABLE(); + _dmaHandle.Init.Channel = DMA_CHANNEL_0; + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA1_Stream4 : DMA1_Stream3; + #endif + } + #endif + #ifdef SPI3_BASE + if (_spiHandle.Instance == SPI3) { + #ifdef STM32F1xx + __HAL_RCC_DMA2_CLK_ENABLE(); + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA2_Channel2 : DMA2_Channel1; + #elif defined(STM32F4xx) + __HAL_RCC_DMA1_CLK_ENABLE(); + _dmaHandle.Init.Channel = DMA_CHANNEL_0; + _dmaHandle.Instance = (direction == DMA_MEMORY_TO_PERIPH) ? DMA1_Stream5 : DMA1_Stream2; + #endif + } + #endif + + HAL_DMA_Init(&_dmaHandle); +} + +byte MarlinSPI::transfer(uint8_t _data) { + uint8_t rxData = 0xFF; + HAL_SPI_TransmitReceive(&_spi.handle, &_data, &rxData, 1, HAL_MAX_DELAY); + return rxData; +} + +__STATIC_INLINE void LL_SPI_EnableDMAReq_RX(SPI_TypeDef *SPIx) { SET_BIT(SPIx->CR2, SPI_CR2_RXDMAEN); } +__STATIC_INLINE void LL_SPI_EnableDMAReq_TX(SPI_TypeDef *SPIx) { SET_BIT(SPIx->CR2, SPI_CR2_TXDMAEN); } + +uint8_t MarlinSPI::dmaTransfer(const void *transmitBuf, void *receiveBuf, uint16_t length) { + const uint8_t ff = 0xFF; + + //if (!LL_SPI_IsEnabled(_spi.handle)) // only enable if disabled + __HAL_SPI_ENABLE(&_spi.handle); + + if (receiveBuf) { + setupDma(_spi.handle, _dmaRx, DMA_PERIPH_TO_MEMORY, true); + HAL_DMA_Start(&_dmaRx, (uint32_t)&(_spi.handle.Instance->DR), (uint32_t)receiveBuf, length); + LL_SPI_EnableDMAReq_RX(_spi.handle.Instance); // Enable Rx DMA Request + } + + // check for 2 lines transfer + bool mincTransmit = true; + if (transmitBuf == nullptr && _spi.handle.Init.Direction == SPI_DIRECTION_2LINES && _spi.handle.Init.Mode == SPI_MODE_MASTER) { + transmitBuf = &ff; + mincTransmit = false; + } + + if (transmitBuf) { + setupDma(_spi.handle, _dmaTx, DMA_MEMORY_TO_PERIPH, mincTransmit); + HAL_DMA_Start(&_dmaTx, (uint32_t)transmitBuf, (uint32_t)&(_spi.handle.Instance->DR), length); + LL_SPI_EnableDMAReq_TX(_spi.handle.Instance); // Enable Tx DMA Request + } + + if (transmitBuf) { + HAL_DMA_PollForTransfer(&_dmaTx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); + HAL_DMA_Abort(&_dmaTx); + HAL_DMA_DeInit(&_dmaTx); + } + + // while ((_spi.handle.Instance->SR & SPI_FLAG_RXNE) != SPI_FLAG_RXNE) {} + + if (receiveBuf) { + HAL_DMA_PollForTransfer(&_dmaRx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); + HAL_DMA_Abort(&_dmaRx); + HAL_DMA_DeInit(&_dmaRx); + } + + return 1; +} + +uint8_t MarlinSPI::dmaSend(const void * transmitBuf, uint16_t length, bool minc) { + setupDma(_spi.handle, _dmaTx, DMA_MEMORY_TO_PERIPH, minc); + HAL_DMA_Start(&_dmaTx, (uint32_t)transmitBuf, (uint32_t)&(_spi.handle.Instance->DR), length); + __HAL_SPI_ENABLE(&_spi.handle); + LL_SPI_EnableDMAReq_TX(_spi.handle.Instance); // Enable Tx DMA Request + HAL_DMA_PollForTransfer(&_dmaTx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); + HAL_DMA_Abort(&_dmaTx); + // DeInit objects + HAL_DMA_DeInit(&_dmaTx); + return 1; +} + +#endif // HAL_STM32 && !STM32H7xx diff --git a/src/HAL/STM32/MarlinSPI.h b/src/HAL/STM32/MarlinSPI.h new file mode 100644 index 0000000..fbd3585 --- /dev/null +++ b/src/HAL/STM32/MarlinSPI.h @@ -0,0 +1,107 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "HAL.h" +#include + +extern "C" { + #include +} + +/** + * Marlin currently requires 3 SPI classes: + * + * SPIClass: + * This class is normally provided by frameworks and has a semi-default interface. + * This is needed because some libraries reference it globally. + * + * SPISettings: + * Container for SPI configs for SPIClass. As above, libraries may reference it globally. + * + * These two classes are often provided by frameworks so we cannot extend them to add + * useful methods for Marlin. + * + * MarlinSPI: + * Provides the default SPIClass interface plus some Marlin goodies such as a simplified + * interface for SPI DMA transfer. + * + */ + +#define DATA_SIZE_8BIT SPI_DATASIZE_8BIT +#define DATA_SIZE_16BIT SPI_DATASIZE_16BIT + +class MarlinSPI { +public: + MarlinSPI() : MarlinSPI(NC, NC, NC, NC) {} + + MarlinSPI(pin_t mosi, pin_t miso, pin_t sclk, pin_t ssel = (pin_t)NC) : _mosiPin(mosi), _misoPin(miso), _sckPin(sclk), _ssPin(ssel) { + _spi.pin_miso = digitalPinToPinName(_misoPin); + _spi.pin_mosi = digitalPinToPinName(_mosiPin); + _spi.pin_sclk = digitalPinToPinName(_sckPin); + _spi.pin_ssel = digitalPinToPinName(_ssPin); + _dataSize = DATA_SIZE_8BIT; + _bitOrder = MSBFIRST; + _dataMode = SPI_MODE_0; + _spi.handle.State = HAL_SPI_STATE_RESET; + setClockDivider(SPI_SPEED_CLOCK_DIV2_MHZ); + } + + void begin(void); + void end(void) {} + + byte transfer(uint8_t _data); + uint8_t dmaTransfer(const void *transmitBuf, void *receiveBuf, uint16_t length); + uint8_t dmaSend(const void * transmitBuf, uint16_t length, bool minc = true); + + /* These methods are deprecated and kept for compatibility. + * Use SPISettings with SPI.beginTransaction() to configure SPI parameters. + */ + void setBitOrder(BitOrder _order) { _bitOrder = _order; } + + void setDataMode(uint8_t _mode) { + switch (_mode) { + case SPI_MODE0: _dataMode = SPI_MODE_0; break; + case SPI_MODE1: _dataMode = SPI_MODE_1; break; + case SPI_MODE2: _dataMode = SPI_MODE_2; break; + case SPI_MODE3: _dataMode = SPI_MODE_3; break; + } + } + + void setClockDivider(uint8_t _div); + +private: + void setupDma(SPI_HandleTypeDef &_spiHandle, DMA_HandleTypeDef &_dmaHandle, uint32_t direction, bool minc = false); + + spi_t _spi; + DMA_HandleTypeDef _dmaTx; + DMA_HandleTypeDef _dmaRx; + BitOrder _bitOrder; + spi_mode_e _dataMode; + uint8_t _clockDivider; + uint32_t _speed; + uint32_t _dataSize; + pin_t _mosiPin; + pin_t _misoPin; + pin_t _sckPin; + pin_t _ssPin; +}; diff --git a/src/HAL/STM32/MarlinSerial.cpp b/src/HAL/STM32/MarlinSerial.cpp new file mode 100644 index 0000000..37a8f40 --- /dev/null +++ b/src/HAL/STM32/MarlinSerial.cpp @@ -0,0 +1,110 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" +#include "MarlinSerial.h" + +#if ENABLED(EMERGENCY_PARSER) + #include "../../feature/e_parser.h" +#endif + +#ifndef USART4 + #define USART4 UART4 +#endif +#ifndef USART5 + #define USART5 UART5 +#endif + +#define DECLARE_SERIAL_PORT(ser_num) \ + void _rx_complete_irq_ ## ser_num (serial_t * obj); \ + MSerialT MSerial ## ser_num (true, USART ## ser_num, &_rx_complete_irq_ ## ser_num); \ + void _rx_complete_irq_ ## ser_num (serial_t * obj) { MSerial ## ser_num ._rx_complete_irq(obj); } + +#if USING_HW_SERIAL1 + DECLARE_SERIAL_PORT(1) +#endif +#if USING_HW_SERIAL2 + DECLARE_SERIAL_PORT(2) +#endif +#if USING_HW_SERIAL3 + DECLARE_SERIAL_PORT(3) +#endif +#if USING_HW_SERIAL4 + DECLARE_SERIAL_PORT(4) +#endif +#if USING_HW_SERIAL5 + DECLARE_SERIAL_PORT(5) +#endif +#if USING_HW_SERIAL6 + DECLARE_SERIAL_PORT(6) +#endif +#if USING_HW_SERIAL7 + DECLARE_SERIAL_PORT(7) +#endif +#if USING_HW_SERIAL8 + DECLARE_SERIAL_PORT(8) +#endif +#if USING_HW_SERIAL9 + DECLARE_SERIAL_PORT(9) +#endif +#if USING_HW_SERIAL10 + DECLARE_SERIAL_PORT(10) +#endif +#if USING_HW_SERIALLP1 + DECLARE_SERIAL_PORT(LP1) +#endif + +void MarlinSerial::begin(unsigned long baud, uint8_t config) { + HardwareSerial::begin(baud, config); + // Replace the IRQ callback with the one we have defined + TERN_(EMERGENCY_PARSER, _serial.rx_callback = _rx_callback); +} + +// This function is Copyright (c) 2006 Nicholas Zambetti. +void MarlinSerial::_rx_complete_irq(serial_t *obj) { + // No Parity error, read byte and store it in the buffer if there is room + unsigned char c; + + if (uart_getc(obj, &c) == 0) { + + rx_buffer_index_t i = (unsigned int)(obj->rx_head + 1) % SERIAL_RX_BUFFER_SIZE; + + // if we should be storing the received character into the location + // just before the tail (meaning that the head would advance to the + // current location of the tail), we're about to overflow the buffer + // and so we don't write the character or advance the head. + if (i != obj->rx_tail) { + obj->rx_buff[obj->rx_head] = c; + obj->rx_head = i; + } + + #if ENABLED(EMERGENCY_PARSER) + emergency_parser.update(static_cast(this)->emergency_state, c); + #endif + } +} + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/MarlinSerial.h b/src/HAL/STM32/MarlinSerial.h new file mode 100644 index 0000000..bf861fb --- /dev/null +++ b/src/HAL/STM32/MarlinSerial.h @@ -0,0 +1,59 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(EMERGENCY_PARSER) + #include "../../feature/e_parser.h" +#endif + +#include "../../core/serial_hook.h" + +typedef void (*usart_rx_callback_t)(serial_t * obj); + +struct MarlinSerial : public HardwareSerial { + MarlinSerial(void *peripheral, usart_rx_callback_t rx_callback) : + HardwareSerial(peripheral), _rx_callback(rx_callback) + { } + + void begin(unsigned long baud, uint8_t config); + inline void begin(unsigned long baud) { begin(baud, SERIAL_8N1); } + + void _rx_complete_irq(serial_t *obj); + +protected: + usart_rx_callback_t _rx_callback; +}; + +typedef Serial1Class MSerialT; +extern MSerialT MSerial1; +extern MSerialT MSerial2; +extern MSerialT MSerial3; +extern MSerialT MSerial4; +extern MSerialT MSerial5; +extern MSerialT MSerial6; +extern MSerialT MSerial7; +extern MSerialT MSerial8; +extern MSerialT MSerial9; +extern MSerialT MSerial10; +extern MSerialT MSerialLP1; diff --git a/src/HAL/STM32/MinSerial.cpp b/src/HAL/STM32/MinSerial.cpp new file mode 100644 index 0000000..b0fcff2 --- /dev/null +++ b/src/HAL/STM32/MinSerial.cpp @@ -0,0 +1,153 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(POSTMORTEM_DEBUGGING) + +#include "../shared/MinSerial.h" + +/* Instruction Synchronization Barrier */ +#define isb() __asm__ __volatile__ ("isb" : : : "memory") + +/* Data Synchronization Barrier */ +#define dsb() __asm__ __volatile__ ("dsb" : : : "memory") + +// Dumb mapping over the registers of a USART device on STM32 +struct USARTMin { + volatile uint32_t SR; + volatile uint32_t DR; + volatile uint32_t BRR; + volatile uint32_t CR1; + volatile uint32_t CR2; +}; + +#if WITHIN(SERIAL_PORT, 1, 6) + // Depending on the CPU, the serial port is different for USART1 + static const uintptr_t regsAddr[] = { + TERN(STM32F1xx, 0x40013800, 0x40011000), // USART1 + 0x40004400, // USART2 + 0x40004800, // USART3 + 0x40004C00, // UART4_BASE + 0x40005000, // UART5_BASE + 0x40011400 // USART6 + }; + static USARTMin * regs = (USARTMin*)regsAddr[SERIAL_PORT - 1]; +#endif + +static void TXBegin() { + #if !WITHIN(SERIAL_PORT, 1, 6) + #warning "Using POSTMORTEM_DEBUGGING requires a physical U(S)ART hardware in case of severe error." + #warning "Disabling the severe error reporting feature currently because the used serial port is not a HW port." + #else + // This is common between STM32F1/STM32F2 and STM32F4 + const int nvicUART[] = { /* NVIC_USART1 */ 37, /* NVIC_USART2 */ 38, /* NVIC_USART3 */ 39, /* NVIC_UART4 */ 52, /* NVIC_UART5 */ 53, /* NVIC_USART6 */ 71 }; + int nvicIndex = nvicUART[SERIAL_PORT - 1]; + + struct NVICMin { + volatile uint32_t ISER[32]; + volatile uint32_t ICER[32]; + }; + + NVICMin *nvicBase = (NVICMin*)0xE000E100; + SBI32(nvicBase->ICER[nvicIndex >> 5], nvicIndex & 0x1F); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + dsb(); + isb(); + + // Example for USART1 disable: (RCC->APB2ENR &= ~(RCC_APB2ENR_USART1EN)) + // Too difficult to reimplement here, let's query the STM32duino macro here + #if SERIAL_PORT == 1 + __HAL_RCC_USART1_CLK_DISABLE(); + __HAL_RCC_USART1_CLK_ENABLE(); + #elif SERIAL_PORT == 2 + __HAL_RCC_USART2_CLK_DISABLE(); + __HAL_RCC_USART2_CLK_ENABLE(); + #elif SERIAL_PORT == 3 + __HAL_RCC_USART3_CLK_DISABLE(); + __HAL_RCC_USART3_CLK_ENABLE(); + #elif SERIAL_PORT == 4 + __HAL_RCC_UART4_CLK_DISABLE(); // BEWARE: UART4 and not USART4 here + __HAL_RCC_UART4_CLK_ENABLE(); + #elif SERIAL_PORT == 5 + __HAL_RCC_UART5_CLK_DISABLE(); // BEWARE: UART5 and not USART5 here + __HAL_RCC_UART5_CLK_ENABLE(); + #elif SERIAL_PORT == 6 + __HAL_RCC_USART6_CLK_DISABLE(); + __HAL_RCC_USART6_CLK_ENABLE(); + #endif + + uint32_t brr = regs->BRR; + regs->CR1 = 0; // Reset the USART + regs->CR2 = 0; // 1 stop bit + + // If we don't touch the BRR (baudrate register), we don't need to recompute. + regs->BRR = brr; + + regs->CR1 = _BV(3) | _BV(13); // 8 bits, no parity, 1 stop bit (TE | UE) + #endif +} + +// A SW memory barrier, to ensure GCC does not overoptimize loops +#define sw_barrier() __asm__ volatile("": : :"memory"); +static void TX(char c) { + #if WITHIN(SERIAL_PORT, 1, 6) + constexpr uint32_t usart_sr_txe = _BV(7); + while (!(regs->SR & usart_sr_txe)) { + hal.watchdog_refresh(); + sw_barrier(); + } + regs->DR = c; + #else + // Let's hope a mystical guru will fix this, one day by writing interrupt-free USB CDC ACM code (or, at least, by polling the registers since interrupt will be queued but will never trigger) + // For now, it's completely lost to oblivion. + #endif +} + +void install_min_serial() { + HAL_min_serial_init = &TXBegin; + HAL_min_serial_out = &TX; +} + +#if NONE(DYNAMIC_VECTORTABLE, STM32F0xx, STM32G0xx) // Cortex M0 can't jump to a symbol that's too far from the current function, so we work around this in exception_arm.cpp +extern "C" { + __attribute__((naked)) void JumpHandler_ASM() { + __asm__ __volatile__ ( + "b CommonHandler_ASM\n" + ); + } + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) HardFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) BusFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) UsageFault_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) MemManage_Handler(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) NMI_Handler(); +} +#endif + +#endif // POSTMORTEM_DEBUGGING +#endif // HAL_STM32 diff --git a/src/HAL/STM32/README.md b/src/HAL/STM32/README.md new file mode 100644 index 0000000..0eb026f --- /dev/null +++ b/src/HAL/STM32/README.md @@ -0,0 +1,11 @@ +# Generic STM32 HAL based on the stm32duino core + +This HAL is intended to act as the generic STM32 HAL for all STM32 chips (The whole F, H and L family). + +Currently it supports: + * STM32F0xx + * STM32F1xx + * STM32F4xx + * STM32F7xx + +Targeting the official [Arduino STM32 Core](https://github.com/stm32duino/Arduino_Core_STM32). diff --git a/src/HAL/STM32/Servo.cpp b/src/HAL/STM32/Servo.cpp new file mode 100644 index 0000000..a00186e --- /dev/null +++ b/src/HAL/STM32/Servo.cpp @@ -0,0 +1,112 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "Servo.h" + +static uint_fast8_t servoCount = 0; +static libServo *servos[NUM_SERVOS] = {0}; +constexpr millis_t servoDelay[] = SERVO_DELAY; +static_assert(COUNT(servoDelay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + +// Initialize to the default timer priority. This will be overridden by a call from timers.cpp. +// This allows all timer interrupt priorities to be managed from a single location in the HAL. +static uint32_t servo_interrupt_priority = NVIC_EncodePriority(NVIC_GetPriorityGrouping(), TIM_IRQ_PRIO, TIM_IRQ_SUBPRIO); + +// This must be called after the STM32 Servo class has initialized the timer. +// It may only be needed after the first call to attach(), but it is possible +// that is is necessary after every detach() call. To be safe this is currently +// called after every call to attach(). +static void fixServoTimerInterruptPriority() { + NVIC_SetPriority(getTimerUpIrq(TIMER_SERVO), servo_interrupt_priority); +} + +libServo::libServo() +: delay(servoDelay[servoCount]), + was_attached_before_pause(false), + value_before_pause(0) +{ + servos[servoCount++] = this; +} + +int8_t libServo::attach(const int pin) { + if (servoCount >= MAX_SERVOS) return -1; + if (pin > 0) servo_pin = pin; + auto result = stm32_servo.attach(servo_pin); + fixServoTimerInterruptPriority(); + return result; +} + +int8_t libServo::attach(const int pin, const int min, const int max) { + if (servoCount >= MAX_SERVOS) return -1; + if (pin > 0) servo_pin = pin; + auto result = stm32_servo.attach(servo_pin, min, max); + fixServoTimerInterruptPriority(); + return result; +} + +void libServo::move(const int value) { + if (attach(0) >= 0) { + stm32_servo.write(value); + safe_delay(delay); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +void libServo::pause() { + was_attached_before_pause = stm32_servo.attached(); + if (was_attached_before_pause) { + value_before_pause = stm32_servo.read(); + stm32_servo.detach(); + } +} + +void libServo::resume() { + if (was_attached_before_pause) { + attach(); + move(value_before_pause); + } +} + +void libServo::pause_all_servos() { + for (auto& servo : servos) + if (servo) servo->pause(); +} + +void libServo::resume_all_servos() { + for (auto& servo : servos) + if (servo) servo->resume(); +} + +void libServo::setInterruptPriority(uint32_t preemptPriority, uint32_t subPriority) { + servo_interrupt_priority = NVIC_EncodePriority(NVIC_GetPriorityGrouping(), preemptPriority, subPriority); +} + +#endif // HAS_SERVOS +#endif // HAL_STM32 diff --git a/src/HAL/STM32/Servo.h b/src/HAL/STM32/Servo.h new file mode 100644 index 0000000..1527e75 --- /dev/null +++ b/src/HAL/STM32/Servo.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +#include "../../core/millis_t.h" + +// Inherit and expand on the official library +class libServo { + public: + libServo(); + int8_t attach(const int pin = 0); // pin == 0 uses value from previous call + int8_t attach(const int pin, const int min, const int max); + void detach() { stm32_servo.detach(); } + int read() { return stm32_servo.read(); } + void move(const int value); + + void pause(); + void resume(); + + static void pause_all_servos(); + static void resume_all_servos(); + static void setInterruptPriority(uint32_t preemptPriority, uint32_t subPriority); + + private: + Servo stm32_servo; + + int servo_pin = 0; + millis_t delay = 0; + + bool was_attached_before_pause; + int value_before_pause; +}; diff --git a/src/HAL/STM32/eeprom_bl24cxx.cpp b/src/HAL/STM32/eeprom_bl24cxx.cpp new file mode 100644 index 0000000..f30b3de --- /dev/null +++ b/src/HAL/STM32/eeprom_bl24cxx.cpp @@ -0,0 +1,85 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +// +// PersistentStore +// + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for IIC_BL24CXX_EEPROM." +#endif + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t * const p = (uint8_t * const)pos; + uint8_t c = eeprom_read_byte(p); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // IIC_BL24CXX_EEPROM +#endif // HAL_STM32 diff --git a/src/HAL/STM32/eeprom_flash.cpp b/src/HAL/STM32/eeprom_flash.cpp new file mode 100644 index 0000000..7c8cc8d --- /dev/null +++ b/src/HAL/STM32/eeprom_flash.cpp @@ -0,0 +1,277 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FLASH_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" + +// Better: "utility/stm32_eeprom.h", but only after updating stm32duino to 2.0.0 +// Use EEPROM.h for compatibility, for now. +#include + +/** + * The STM32 HAL supports chips that deal with "pages" and some with "sectors" and some that + * even have multiple "banks" of flash. + * + * This code is a bit of a mashup of + * framework-arduinoststm32/cores/arduino/stm32/stm32_eeprom.c + * hal/hal_lpc1768/persistent_store_flash.cpp + * + * This has only be written against those that use a single "sector" design. + * + * Those that deal with "pages" could be made to work. Looking at the STM32F07 for example, there are + * 128 "pages", each 2kB in size. If we continued with our EEPROM being 4Kb, we'd always need to operate + * on 2 of these pages. Each write, we'd use 2 different pages from a pool of pages until we are done. + */ + +#if ENABLED(FLASH_EEPROM_LEVELING) + + #include "stm32_def.h" + + #define DEBUG_OUT ENABLED(EEPROM_CHITCHAT) + #include "../../core/debug_out.h" + + #ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB + #endif + + #ifndef FLASH_SECTOR + #define FLASH_SECTOR (FLASH_SECTOR_TOTAL - 1) + #endif + #ifndef FLASH_UNIT_SIZE + #define FLASH_UNIT_SIZE 0x20000 // 128kB + #endif + + #ifndef FLASH_ADDRESS_START + #define FLASH_ADDRESS_START (FLASH_END - ((FLASH_SECTOR_TOTAL - (FLASH_SECTOR)) * (FLASH_UNIT_SIZE)) + 1) + #endif + #define FLASH_ADDRESS_END (FLASH_ADDRESS_START + FLASH_UNIT_SIZE - 1) + + #define EEPROM_SLOTS ((FLASH_UNIT_SIZE) / (MARLIN_EEPROM_SIZE)) + #define SLOT_ADDRESS(slot) (FLASH_ADDRESS_START + (slot * (MARLIN_EEPROM_SIZE))) + + #define UNLOCK_FLASH() if (!flash_unlocked) { \ + HAL_FLASH_Unlock(); \ + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | \ + FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); \ + flash_unlocked = true; \ + } + #define LOCK_FLASH() if (flash_unlocked) { HAL_FLASH_Lock(); flash_unlocked = false; } + + #define EMPTY_UINT32 ((uint32_t)-1) + #define EMPTY_UINT8 ((uint8_t)-1) + + static uint8_t ram_eeprom[MARLIN_EEPROM_SIZE] __attribute__((aligned(4))) = {0}; + static int current_slot = -1; + + static_assert(0 == MARLIN_EEPROM_SIZE % 4, "MARLIN_EEPROM_SIZE must be a multiple of 4"); // Ensure copying as uint32_t is safe + static_assert(0 == FLASH_UNIT_SIZE % MARLIN_EEPROM_SIZE, "MARLIN_EEPROM_SIZE must divide evenly into your FLASH_UNIT_SIZE"); + static_assert(FLASH_UNIT_SIZE >= MARLIN_EEPROM_SIZE, "FLASH_UNIT_SIZE must be greater than or equal to your MARLIN_EEPROM_SIZE"); + static_assert(IS_FLASH_SECTOR(FLASH_SECTOR), "FLASH_SECTOR is invalid"); + static_assert(IS_POWER_OF_2(FLASH_UNIT_SIZE), "FLASH_UNIT_SIZE should be a power of 2, please check your chip's spec sheet"); + +#endif + +static bool eeprom_data_written = false; + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { + + EEPROM.begin(); // Avoid STM32 EEPROM.h warning (do nothing) + + #if ENABLED(FLASH_EEPROM_LEVELING) + + if (current_slot == -1 || eeprom_data_written) { + // This must be the first time since power on that we have accessed the storage, or someone + // loaded and called write_data and never called access_finish. + // Lets go looking for the slot that holds our configuration. + if (eeprom_data_written) DEBUG_ECHOLNPGM("Dangling EEPROM write_data"); + uint32_t address = FLASH_ADDRESS_START; + while (address <= FLASH_ADDRESS_END) { + uint32_t address_value = (*(__IO uint32_t*)address); + if (address_value != EMPTY_UINT32) { + current_slot = (address - (FLASH_ADDRESS_START)) / (MARLIN_EEPROM_SIZE); + break; + } + address += sizeof(uint32_t); + } + if (current_slot == -1) { + // We didn't find anything, so we'll just initialize to empty + for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = EMPTY_UINT8; + current_slot = EEPROM_SLOTS; + } + else { + // load current settings + uint8_t *eeprom_data = (uint8_t *)SLOT_ADDRESS(current_slot); + for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = eeprom_data[i]; + DEBUG_ECHOLNPGM("EEPROM loaded from slot ", current_slot, "."); + } + eeprom_data_written = false; + } + + #else + eeprom_buffer_fill(); + #endif + + return true; +} + +bool PersistentStore::access_finish() { + + if (eeprom_data_written) { + #ifdef STM32F4xx + // MCU may come up with flash error bits which prevent some flash operations. + // Clear flags prior to flash operations to prevent errors. + __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR); + #endif + + #if ENABLED(FLASH_EEPROM_LEVELING) + + HAL_StatusTypeDef status = HAL_ERROR; + bool flash_unlocked = false; + + if (--current_slot < 0) { + // all slots have been used, erase everything and start again + + FLASH_EraseInitTypeDef EraseInitStruct; + uint32_t SectorError = 0; + + EraseInitStruct.TypeErase = FLASH_TYPEERASE_SECTORS; + EraseInitStruct.VoltageRange = FLASH_VOLTAGE_RANGE_3; + EraseInitStruct.Sector = FLASH_SECTOR; + EraseInitStruct.NbSectors = 1; + + current_slot = EEPROM_SLOTS - 1; + UNLOCK_FLASH(); + + TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT()); + hal.isr_off(); + status = HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError); + hal.isr_on(); + TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT()); + if (status != HAL_OK) { + DEBUG_ECHOLNPGM("HAL_FLASHEx_Erase=", status); + DEBUG_ECHOLNPGM("GetError=", HAL_FLASH_GetError()); + DEBUG_ECHOLNPGM("SectorError=", SectorError); + LOCK_FLASH(); + return false; + } + } + + UNLOCK_FLASH(); + + uint32_t offset = 0; + uint32_t address = SLOT_ADDRESS(current_slot); + uint32_t address_end = address + MARLIN_EEPROM_SIZE; + uint32_t data = 0; + + bool success = true; + + while (address < address_end) { + memcpy(&data, ram_eeprom + offset, sizeof(uint32_t)); + status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, data); + if (status == HAL_OK) { + address += sizeof(uint32_t); + offset += sizeof(uint32_t); + } + else { + DEBUG_ECHOLNPGM("HAL_FLASH_Program=", status); + DEBUG_ECHOLNPGM("GetError=", HAL_FLASH_GetError()); + DEBUG_ECHOLNPGM("address=", address); + success = false; + break; + } + } + + LOCK_FLASH(); + + if (success) { + eeprom_data_written = false; + DEBUG_ECHOLNPGM("EEPROM saved to slot ", current_slot, "."); + } + + return success; + + #else + // The following was written for the STM32F4 but may work with other MCUs as well. + // Most STM32F4 flash does not allow reading from flash during erase operations. + // This takes about a second on a STM32F407 with a 128kB sector used as EEPROM. + // Interrupts during this time can have unpredictable results, such as killing Servo + // output. Servo output still glitches with interrupts disabled, but recovers after the + // erase. + TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT()); + hal.isr_off(); + eeprom_buffer_flush(); + hal.isr_on(); + TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT()); + + eeprom_data_written = false; + #endif + } + + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + while (size--) { + uint8_t v = *value; + #if ENABLED(FLASH_EEPROM_LEVELING) + if (v != ram_eeprom[pos]) { + ram_eeprom[pos] = v; + eeprom_data_written = true; + } + #else + if (v != eeprom_buffered_read_byte(pos)) { + eeprom_buffered_write_byte(pos, v); + eeprom_data_written = true; + } + #endif + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + const uint8_t c = TERN(FLASH_EEPROM_LEVELING, ram_eeprom[pos], eeprom_buffered_read_byte(pos)); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // FLASH_EEPROM_EMULATION +#endif // HAL_STM32 diff --git a/src/HAL/STM32/eeprom_if_iic.cpp b/src/HAL/STM32/eeprom_if_iic.cpp new file mode 100644 index 0000000..ad8712c --- /dev/null +++ b/src/HAL/STM32/eeprom_if_iic.cpp @@ -0,0 +1,56 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +/** + * Platform-independent Arduino functions for I2C EEPROM. + * Enable USE_SHARED_EEPROM if not supplied by the framework. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../../libs/BL24CXX.h" +#include "../shared/eeprom_if.h" + +void eeprom_init() { BL24CXX::init(); } + +// ------------------------ +// Public functions +// ------------------------ + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::writeOneByte(eeprom_address, value); +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::readOneByte(eeprom_address); +} + +#endif // IIC_BL24CXX_EEPROM +#endif // HAL_STM32 diff --git a/src/HAL/STM32/eeprom_sdcard.cpp b/src/HAL/STM32/eeprom_sdcard.cpp new file mode 100644 index 0000000..473b656 --- /dev/null +++ b/src/HAL/STM32/eeprom_sdcard.cpp @@ -0,0 +1,94 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +/** + * Implementation of EEPROM settings in SD Card + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SDCARD_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" +#include "../../sd/cardreader.h" + +#define EEPROM_FILENAME "eeprom.dat" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +#define _ALIGN(x) __attribute__ ((aligned(x))) +static char _ALIGN(4) HAL_eeprom_data[MARLIN_EEPROM_SIZE]; + +bool PersistentStore::access_start() { + if (!card.isMounted()) return false; + + SdFile file, root = card.getroot(); + if (!file.open(&root, EEPROM_FILENAME, O_RDONLY)) + return true; + + int bytes_read = file.read(HAL_eeprom_data, MARLIN_EEPROM_SIZE); + if (bytes_read < 0) return false; + for (; bytes_read < MARLIN_EEPROM_SIZE; bytes_read++) + HAL_eeprom_data[bytes_read] = 0xFF; + file.close(); + return true; +} + +bool PersistentStore::access_finish() { + if (!card.isMounted()) return false; + + SdFile file, root = card.getroot(); + int bytes_written = 0; + if (file.open(&root, EEPROM_FILENAME, O_CREAT | O_WRITE | O_TRUNC)) { + bytes_written = file.write(HAL_eeprom_data, MARLIN_EEPROM_SIZE); + file.close(); + } + return (bytes_written == MARLIN_EEPROM_SIZE); +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + for (size_t i = 0; i < size; i++) + HAL_eeprom_data[pos + i] = value[i]; + crc16(crc, value, size); + pos += size; + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uint16_t *crc, const bool writing/*=true*/) { + for (size_t i = 0; i < size; i++) { + uint8_t c = HAL_eeprom_data[pos + i]; + if (writing) value[i] = c; + crc16(crc, &c, 1); + } + pos += size; + return false; +} + +#endif // SDCARD_EEPROM_EMULATION +#endif // HAL_STM32 diff --git a/src/HAL/STM32/eeprom_sram.cpp b/src/HAL/STM32/eeprom_sram.cpp new file mode 100644 index 0000000..687e7f5 --- /dev/null +++ b/src/HAL/STM32/eeprom_sram.cpp @@ -0,0 +1,70 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SRAM_EEPROM_EMULATION) + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + while (size--) { + uint8_t v = *value; + + // Save to Backup SRAM + *(__IO uint8_t *)(BKPSRAM_BASE + (uint8_t * const)pos) = v; + + crc16(crc, &v, 1); + pos++; + value++; + }; + + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + // Read from either external EEPROM, program flash or Backup SRAM + const uint8_t c = ( *(__IO uint8_t *)(BKPSRAM_BASE + ((uint8_t*)pos)) ); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // SRAM_EEPROM_EMULATION +#endif // HAL_STM32 diff --git a/src/HAL/STM32/eeprom_wired.cpp b/src/HAL/STM32/eeprom_wired.cpp new file mode 100644 index 0000000..cf04681 --- /dev/null +++ b/src/HAL/STM32/eeprom_wired.cpp @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + // Read from either external EEPROM, program flash or Backup SRAM + const uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // HAL_STM32 diff --git a/src/HAL/STM32/endstop_interrupts.h b/src/HAL/STM32/endstop_interrupts.h new file mode 100644 index 0000000..9087088 --- /dev/null +++ b/src/HAL/STM32/endstop_interrupts.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(P, endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/STM32/fast_pwm.cpp b/src/HAL/STM32/fast_pwm.cpp new file mode 100644 index 0000000..a0d8ecc --- /dev/null +++ b/src/HAL/STM32/fast_pwm.cpp @@ -0,0 +1,88 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +// Array to support sticky frequency sets per timer +static uint16_t timer_freq[TIMER_NUM]; + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + const uint16_t duty = invert ? v_size - v : v; + if (PWM_PIN(pin)) { + const PinName pin_name = digitalPinToPinName(pin); + TIM_TypeDef * const Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); + + const timer_index_t index = get_timer_index(Instance); + const bool needs_freq = (HardwareTimer_Handle[index] == nullptr); + if (needs_freq) // A new instance must be set to the default frequency of PWM_FREQUENCY + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM)); + + HardwareTimer * const HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + const uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin_name, PinMap_PWM)); + const TimerModes_t previousMode = HT->getMode(channel); + if (previousMode != TIMER_OUTPUT_COMPARE_PWM1) + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + + if (needs_freq && timer_freq[index] == 0) // If the timer is unconfigured and no freq is set then default PWM_FREQUENCY + set_pwm_frequency(pin_name, PWM_FREQUENCY); // Set the frequency and save the value to the assigned index no. + + // Note the resolution is sticky here, the input can be upto 16 bits and that would require RESOLUTION_16B_COMPARE_FORMAT (16) + // If such a need were to manifest then we would need to calc the resolution based on the v_size parameter and add code for it. + HT->setCaptureCompare(channel, duty, RESOLUTION_8B_COMPARE_FORMAT); // Set the duty, the calc is done in the library :) + pinmap_pinout(pin_name, PinMap_PWM); // Make sure the pin output state is set. + if (previousMode != TIMER_OUTPUT_COMPARE_PWM1) HT->resume(); + } + else { + pinMode(pin, OUTPUT); + digitalWrite(pin, duty < v_size / 2 ? LOW : HIGH); + } +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer + const PinName pin_name = digitalPinToPinName(pin); + TIM_TypeDef * const Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); // Get HAL timer instance + const timer_index_t index = get_timer_index(Instance); + + // Protect used timers. + #ifdef STEP_TIMER + if (index == TIMER_INDEX(STEP_TIMER)) return; + #endif + #ifdef TEMP_TIMER + if (index == TIMER_INDEX(TEMP_TIMER)) return; + #endif + #if defined(PULSE_TIMER) && MF_TIMER_PULSE != MF_TIMER_STEP + if (index == TIMER_INDEX(PULSE_TIMER)) return; + #endif + + if (HardwareTimer_Handle[index] == nullptr) // If frequency is set before duty we need to create a handle here. + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM)); + HardwareTimer * const HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + HT->setOverflow(f_desired, HERTZ_FORMAT); + timer_freq[index] = f_desired; // Save the last frequency so duty will not set the default for this timer number. +} + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/fastio.cpp b/src/HAL/STM32/fastio.cpp new file mode 100644 index 0000000..b34555b --- /dev/null +++ b/src/HAL/STM32/fastio.cpp @@ -0,0 +1,36 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +GPIO_TypeDef* FastIOPortMap[LastPort + 1] = { 0 }; + +void FastIO_init() { + LOOP_L_N(i, NUM_DIGITAL_PINS) + FastIOPortMap[STM_PORT(digitalPin[i])] = get_GPIO_Port(STM_PORT(digitalPin[i])); +} + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/fastio.h b/src/HAL/STM32/fastio.h new file mode 100644 index 0000000..4a48954 --- /dev/null +++ b/src/HAL/STM32/fastio.h @@ -0,0 +1,91 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O interfaces for STM32 + * These use GPIO register access for fast port manipulation. + */ + +// ------------------------ +// Public Variables +// ------------------------ + +extern GPIO_TypeDef * FastIOPortMap[]; + +// ------------------------ +// Public functions +// ------------------------ + +void FastIO_init(); // Must be called before using fast io macros +#define FASTIO_INIT() FastIO_init() + +// ------------------------ +// Defines +// ------------------------ + +#define _BV32(b) (1UL << (b)) + +#ifndef PWM + #define PWM OUTPUT +#endif + +#if defined(STM32F0xx) || defined(STM32F1xx) || defined(STM32F3xx) || defined(STM32L0xx) || defined(STM32L4xx) + #define _WRITE(IO, V) do { \ + if (V) FastIOPortMap[STM_PORT(digitalPinToPinName(IO))]->BSRR = _BV32(STM_PIN(digitalPinToPinName(IO))) ; \ + else FastIOPortMap[STM_PORT(digitalPinToPinName(IO))]->BRR = _BV32(STM_PIN(digitalPinToPinName(IO))) ; \ + }while(0) +#else + #define _WRITE(IO, V) (FastIOPortMap[STM_PORT(digitalPinToPinName(IO))]->BSRR = _BV32(STM_PIN(digitalPinToPinName(IO)) + ((V) ? 0 : 16))) +#endif + +#define _READ(IO) bool(READ_BIT(FastIOPortMap[STM_PORT(digitalPinToPinName(IO))]->IDR, _BV32(STM_PIN(digitalPinToPinName(IO))))) +#define _TOGGLE(IO) TBI32(FastIOPortMap[STM_PORT(digitalPinToPinName(IO))]->ODR, STM_PIN(digitalPinToPinName(IO))) + +#define _GET_MODE(IO) +#define _SET_MODE(IO,M) pinMode(IO, M) +#define _SET_OUTPUT(IO) pinMode(IO, OUTPUT) //!< Output Push Pull Mode & GPIO_NOPULL +#define _SET_OUTPUT_OD(IO) pinMode(IO, OUTPUT_OPEN_DRAIN) + +#define WRITE(IO,V) _WRITE(IO,V) +#define READ(IO) _READ(IO) +#define TOGGLE(IO) _TOGGLE(IO) + +#define OUT_WRITE(IO,V) do{ _SET_OUTPUT(IO); WRITE(IO,V); }while(0) +#define OUT_WRITE_OD(IO,V) do{ _SET_OUTPUT_OD(IO); WRITE(IO,V); }while(0) + +#define SET_INPUT(IO) _SET_MODE(IO, INPUT) //!< Input Floating Mode +#define SET_INPUT_PULLUP(IO) _SET_MODE(IO, INPUT_PULLUP) //!< Input with Pull-up activation +#define SET_INPUT_PULLDOWN(IO) _SET_MODE(IO, INPUT_PULLDOWN) //!< Input with Pull-down activation +#define SET_OUTPUT(IO) OUT_WRITE(IO, LOW) +#define SET_PWM(IO) _SET_MODE(IO, PWM) + +#define IS_INPUT(IO) +#define IS_OUTPUT(IO) + +#define PWM_PIN(P) digitalPinHasPWM(P) +#define NO_COMPILE_TIME_PWM + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/src/HAL/STM32/inc/Conditionals_LCD.h b/src/HAL/STM32/inc/Conditionals_LCD.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/STM32/inc/Conditionals_LCD.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/STM32/inc/Conditionals_adv.h b/src/HAL/STM32/inc/Conditionals_adv.h new file mode 100644 index 0000000..451c94f --- /dev/null +++ b/src/HAL/STM32/inc/Conditionals_adv.h @@ -0,0 +1,35 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if BOTH(SDSUPPORT, USBD_USE_CDC_MSC) && DISABLED(NO_SD_HOST_DRIVE) + #define HAS_SD_HOST_DRIVE 1 +#endif + +// Fix F_CPU not being a compile-time constant in STSTM32 framework +#ifdef BOARD_F_CPU + #undef F_CPU + #define F_CPU BOARD_F_CPU +#endif + +// The Sensitive Pins array is not optimizable +#define RUNTIME_ONLY_ANALOG_TO_DIGITAL diff --git a/src/HAL/STM32/inc/Conditionals_post.h b/src/HAL/STM32/inc/Conditionals_post.h new file mode 100644 index 0000000..18826e1 --- /dev/null +++ b/src/HAL/STM32/inc/Conditionals_post.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// If no real or emulated EEPROM selected, fall back to SD emulation +#if USE_FALLBACK_EEPROM + #define SDCARD_EEPROM_EMULATION +#elif EITHER(I2C_EEPROM, SPI_EEPROM) + #define USE_SHARED_EEPROM 1 +#endif diff --git a/src/HAL/STM32/inc/SanityCheck.h b/src/HAL/STM32/inc/SanityCheck.h new file mode 100644 index 0000000..0f1a2ac --- /dev/null +++ b/src/HAL/STM32/inc/SanityCheck.h @@ -0,0 +1,57 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test STM32-specific configuration values for errors at compile-time. + */ +//#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) +// #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" +//#endif + + +#if ENABLED(SDCARD_EEPROM_EMULATION) && DISABLED(SDSUPPORT) + #undef SDCARD_EEPROM_EMULATION // Avoid additional error noise + #if USE_FALLBACK_EEPROM + #warning "EEPROM type not specified. Fallback is SDCARD_EEPROM_EMULATION." + #endif + #error "SDCARD_EEPROM_EMULATION requires SDSUPPORT. Enable SDSUPPORT or choose another EEPROM emulation." +#endif + +#if defined(STM32F4xx) && BOTH(PRINTCOUNTER, FLASH_EEPROM_EMULATION) + #warning "FLASH_EEPROM_EMULATION may cause long delays when writing and should not be used while printing." + #error "Disable PRINTCOUNTER or choose another EEPROM emulation." +#endif + +#if !defined(STM32F4xx) && ENABLED(FLASH_EEPROM_LEVELING) + #error "FLASH_EEPROM_LEVELING is currently only supported on STM32F4 hardware." +#endif + +#if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + #error "SERIAL_STATS_MAX_RX_QUEUED is not supported on STM32." +#elif ENABLED(SERIAL_STATS_DROPPED_RX) + #error "SERIAL_STATS_DROPPED_RX is not supported on STM32." +#endif + +#if ANY(TFT_COLOR_UI, TFT_LVGL_UI, TFT_CLASSIC_UI) && NOT_TARGET(STM32H7xx, STM32F4xx, STM32F1xx) + #error "TFT_COLOR_UI, TFT_LVGL_UI and TFT_CLASSIC_UI are currently only supported on STM32H7, STM32F4 and STM32F1 hardware." +#endif diff --git a/src/HAL/STM32/msc_sd.cpp b/src/HAL/STM32/msc_sd.cpp new file mode 100644 index 0000000..a40bec9 --- /dev/null +++ b/src/HAL/STM32/msc_sd.cpp @@ -0,0 +1,130 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] + * + * This program 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_SD_HOST_DRIVE + +#include "../shared/Marduino.h" +#include "msc_sd.h" +#include "usbd_core.h" + +#include "../../sd/cardreader.h" + +#include +#include + +#define BLOCK_SIZE 512 +#define PRODUCT_ID 0x29 + +class Sd2CardUSBMscHandler : public USBMscHandler { +public: + DiskIODriver* diskIODriver() { + #if ENABLED(MULTI_VOLUME) + #if SHARED_VOLUME_IS(SD_ONBOARD) + return &card.media_driver_sdcard; + #elif SHARED_VOLUME_IS(USB_FLASH_DRIVE) + return &card.media_driver_usbFlash; + #endif + #else + return card.diskIODriver(); + #endif + } + + bool GetCapacity(uint32_t *pBlockNum, uint16_t *pBlockSize) { + *pBlockNum = diskIODriver()->cardSize(); + *pBlockSize = BLOCK_SIZE; + return true; + } + + bool Write(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) { + auto sd2card = diskIODriver(); + // single block + if (blkLen == 1) { + hal.watchdog_refresh(); + sd2card->writeBlock(blkAddr, pBuf); + return true; + } + + // multi block optimization + sd2card->writeStart(blkAddr, blkLen); + while (blkLen--) { + hal.watchdog_refresh(); + sd2card->writeData(pBuf); + pBuf += BLOCK_SIZE; + } + sd2card->writeStop(); + return true; + } + + bool Read(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) { + auto sd2card = diskIODriver(); + // single block + if (blkLen == 1) { + hal.watchdog_refresh(); + sd2card->readBlock(blkAddr, pBuf); + return true; + } + + // multi block optimization + sd2card->readStart(blkAddr); + while (blkLen--) { + hal.watchdog_refresh(); + sd2card->readData(pBuf); + pBuf += BLOCK_SIZE; + } + sd2card->readStop(); + return true; + } + + bool IsReady() { + return diskIODriver()->isReady(); + } +}; + +Sd2CardUSBMscHandler usbMscHandler; + +/* USB Mass storage Standard Inquiry Data */ +uint8_t Marlin_STORAGE_Inquirydata[] = { /* 36 */ + /* LUN 0 */ + 0x00, + 0x80, + 0x02, + 0x02, + (STANDARD_INQUIRY_DATA_LEN - 5), + 0x00, + 0x00, + 0x00, + 'M', 'A', 'R', 'L', 'I', 'N', ' ', ' ', /* Manufacturer : 8 bytes */ + 'P', 'r', 'o', 'd', 'u', 'c', 't', ' ', /* Product : 16 Bytes */ + ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', + '0', '.', '0', '1', /* Version : 4 Bytes */ +}; + +USBMscHandler *pSingleMscHandler = &usbMscHandler; + +void MSC_SD_init() { + USBDevice.end(); + delay(200); + USBDevice.registerMscHandlers(1, &pSingleMscHandler, Marlin_STORAGE_Inquirydata); + USBDevice.begin(); +} + +#endif // HAS_SD_HOST_DRIVE +#endif // HAL_STM32 diff --git a/src/HAL/STM32/msc_sd.h b/src/HAL/STM32/msc_sd.h new file mode 100644 index 0000000..a8e5349 --- /dev/null +++ b/src/HAL/STM32/msc_sd.h @@ -0,0 +1,18 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] + * + * This program 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +void MSC_SD_init(); diff --git a/src/HAL/STM32/pinsDebug.h b/src/HAL/STM32/pinsDebug.h new file mode 100644 index 0000000..55c64c8 --- /dev/null +++ b/src/HAL/STM32/pinsDebug.h @@ -0,0 +1,283 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +#ifndef NUM_DIGITAL_PINS + // Only in ST's Arduino core (STM32duino, STM32Core) + #error "Expected NUM_DIGITAL_PINS not found" +#endif + +/** + * Life gets complicated if you want an easy to use 'M43 I' output (in port/pin order) + * because the variants in this platform do not always define all the I/O port/pins + * that a CPU has. + * + * VARIABLES: + * Ard_num - Arduino pin number - defined by the platform. It is used by digitalRead and + * digitalWrite commands and by M42. + * - does not contain port/pin info + * - is not in port/pin order + * - typically a variant will only assign Ard_num to port/pins that are actually used + * Index - M43 counter - only used to get Ard_num + * x - a parameter/argument used to search the pin_array to try to find a signal name + * associated with a Ard_num + * Port_pin - port number and pin number for use with CPU registers and printing reports + * + * Since M43 uses digitalRead and digitalWrite commands, only the Port_pins with an Ard_num + * are accessed and/or displayed. + * + * Three arrays are used. + * + * digitalPin[] is provided by the platform. It consists of the Port_pin numbers in + * Arduino pin number order. + * + * pin_array is a structure generated by the pins/pinsDebug.h header file. It is generated by + * the preprocessor. Only the signals associated with enabled options are in this table. + * It contains: + * - name of the signal + * - the Ard_num assigned by the pins_YOUR_BOARD.h file using the platform defines. + * EXAMPLE: "#define KILL_PIN PB1" results in Ard_num of 57. 57 is then used as the + * argument to digitalPinToPinName(IO) to get the Port_pin number + * - if it is a digital or analog signal. PWMs are considered digital here. + * + * pin_xref is a structure generated by this header file. It is generated by the + * preprocessor. It is in port/pin order. It contains just the port/pin numbers defined by the + * platform for this variant. + * - Ard_num + * - printable version of Port_pin + * + * Routines with an "x" as a parameter/argument are used to search the pin_array to try to + * find a signal name associated with a port/pin. + * + * NOTE - the Arduino pin number is what is used by the M42 command, NOT the port/pin for that + * signal. The Arduino pin number is listed by the M43 I command. + */ + +//////////////////////////////////////////////////////// +// +// make a list of the Arduino pin numbers in the Port/Pin order +// + +#define _PIN_ADD(NAME_ALPHA, ARDUINO_NUM) { NAME_ALPHA, ARDUINO_NUM }, +#define PIN_ADD(NAME) _PIN_ADD(#NAME, NAME) + +typedef struct { + char Port_pin_alpha[5]; + pin_t Ard_num; +} XrefInfo; + +const XrefInfo pin_xref[] PROGMEM = { + #include "pins_Xref.h" +}; + +//////////////////////////////////////////////////////////// + +#define MODE_PIN_INPUT 0 // Input mode (reset state) +#define MODE_PIN_OUTPUT 1 // General purpose output mode +#define MODE_PIN_ALT 2 // Alternate function mode +#define MODE_PIN_ANALOG 3 // Analog mode + +#define PIN_NUM(P) (P & 0x000F) +#define PIN_NUM_ALPHA_LEFT(P) (((P & 0x000F) < 10) ? ('0' + (P & 0x000F)) : '1') +#define PIN_NUM_ALPHA_RIGHT(P) (((P & 0x000F) > 9) ? ('0' + (P & 0x000F) - 10) : 0 ) +#define PORT_NUM(P) ((P >> 4) & 0x0007) +#define PORT_ALPHA(P) ('A' + (P >> 4)) + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#if PA0 >= NUM_DIGITAL_PINS + #define HAS_HIGH_ANALOG_PINS 1 +#endif +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS + TERN0(HAS_HIGH_ANALOG_PINS, NUM_ANALOG_INPUTS) +#define VALID_PIN(ANUM) ((ANUM) >= 0 && (ANUM) < NUMBER_PINS_TOTAL) +#define digitalRead_mod(Ard_num) extDigitalRead(Ard_num) // must use Arduino pin numbers when doing reads +#define PRINT_PIN(Q) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PORT(ANUM) port_print(ANUM) +#define DIGITAL_PIN_TO_ANALOG_PIN(ANUM) -1 // will report analog pin number in the print port routine + +// x is a variable used to search pin_array +#define GET_ARRAY_IS_DIGITAL(x) ((bool) pin_array[x].is_digital) +#define GET_ARRAY_PIN(x) ((pin_t) pin_array[x].pin) +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 33 // space needed to be pretty if not first name assigned to a pin + +// +// Pin Mapping for M43 +// +#define GET_PIN_MAP_PIN_M43(Index) pin_xref[Index].Ard_num + +#ifndef M43_NEVER_TOUCH + #define _M43_NEVER_TOUCH(Index) (Index >= 9 && Index <= 12) // SERIAL/USB pins: PA9(TX) PA10(RX) PA11(USB_DM) PA12(USB_DP) + #ifdef KILL_PIN + #define M43_NEVER_TOUCH(Index) m43_never_touch(Index) + + bool m43_never_touch(const pin_t Index) { + static pin_t M43_kill_index = -1; + if (M43_kill_index < 0) + for (M43_kill_index = 0; M43_kill_index < NUMBER_PINS_TOTAL; M43_kill_index++) + if (KILL_PIN == GET_PIN_MAP_PIN_M43(M43_kill_index)) break; + return _M43_NEVER_TOUCH(Index) || Index == M43_kill_index; // KILL_PIN and SERIAL/USB + } + #else + #define M43_NEVER_TOUCH(Index) _M43_NEVER_TOUCH(Index) + #endif +#endif + +uint8_t get_pin_mode(const pin_t Ard_num) { + const PinName dp = digitalPinToPinName(Ard_num); + uint32_t ll_pin = STM_LL_GPIO_PIN(dp); + GPIO_TypeDef *port = get_GPIO_Port(STM_PORT(dp)); + uint32_t mode = LL_GPIO_GetPinMode(port, ll_pin); + switch (mode) { + case LL_GPIO_MODE_ANALOG: return MODE_PIN_ANALOG; + case LL_GPIO_MODE_INPUT: return MODE_PIN_INPUT; + case LL_GPIO_MODE_OUTPUT: return MODE_PIN_OUTPUT; + case LL_GPIO_MODE_ALTERNATE: return MODE_PIN_ALT; + TERN_(STM32F1xx, case LL_GPIO_MODE_FLOATING:) + default: return 0; + } +} + +bool GET_PINMODE(const pin_t Ard_num) { + const uint8_t pin_mode = get_pin_mode(Ard_num); + return pin_mode == MODE_PIN_OUTPUT || pin_mode == MODE_PIN_ALT; // assume all alt definitions are PWM +} + +int8_t digital_pin_to_analog_pin(const pin_t Ard_num) { + if (WITHIN(Ard_num, NUM_ANALOG_FIRST, NUM_ANALOG_FIRST + NUM_ANALOG_INPUTS - 1)) + return Ard_num - NUM_ANALOG_FIRST; + + const uint32_t ind = digitalPinToAnalogInput(Ard_num); + return (ind < NUM_ANALOG_INPUTS) ? ind : -1; +} + +bool IS_ANALOG(const pin_t Ard_num) { + return get_pin_mode(Ard_num) == MODE_PIN_ANALOG; +} + +bool is_digital(const pin_t Ard_num) { + const uint8_t pin_mode = get_pin_mode(pin_array[Ard_num].pin); + return pin_mode == MODE_PIN_INPUT || pin_mode == MODE_PIN_OUTPUT; +} + +void port_print(const pin_t Ard_num) { + char buffer[16]; + pin_t Index; + for (Index = 0; Index < NUMBER_PINS_TOTAL; Index++) + if (Ard_num == GET_PIN_MAP_PIN_M43(Index)) break; + + const char * ppa = pin_xref[Index].Port_pin_alpha; + sprintf_P(buffer, PSTR("%s"), ppa); + SERIAL_ECHO(buffer); + if (ppa[3] == '\0') SERIAL_CHAR(' '); + + // print analog pin number + const int8_t Port_pin = digital_pin_to_analog_pin(Ard_num); + if (Port_pin >= 0) { + sprintf_P(buffer, PSTR(" (A%d) "), Port_pin); + SERIAL_ECHO(buffer); + if (Port_pin < 10) SERIAL_CHAR(' '); + } + else + SERIAL_ECHO_SP(7); + + // Print number to be used with M42 + int calc_p = Ard_num % (NUM_DIGITAL_PINS + 1); + if (Ard_num > NUM_DIGITAL_PINS && calc_p > 7) calc_p += 8; + SERIAL_ECHOPGM(" M42 P", calc_p); + SERIAL_CHAR(' '); + if (calc_p < 100) { + SERIAL_CHAR(' '); + if (calc_p < 10) + SERIAL_CHAR(' '); + } +} + +bool pwm_status(const pin_t Ard_num) { + return get_pin_mode(Ard_num) == MODE_PIN_ALT; +} + +void pwm_details(const pin_t Ard_num) { + #ifndef STM32F1xx + if (pwm_status(Ard_num)) { + uint32_t alt_all = 0; + const PinName dp = digitalPinToPinName(Ard_num); + pin_t pin_number = uint8_t(PIN_NUM(dp)); + const bool over_7 = pin_number >= 8; + const uint8_t ind = over_7 ? 1 : 0; + switch (PORT_ALPHA(dp)) { // get alt function + case 'A' : alt_all = GPIOA->AFR[ind]; break; + case 'B' : alt_all = GPIOB->AFR[ind]; break; + case 'C' : alt_all = GPIOC->AFR[ind]; break; + case 'D' : alt_all = GPIOD->AFR[ind]; break; + #ifdef PE_0 + case 'E' : alt_all = GPIOE->AFR[ind]; break; + #elif defined(PF_0) + case 'F' : alt_all = GPIOF->AFR[ind]; break; + #elif defined(PG_0) + case 'G' : alt_all = GPIOG->AFR[ind]; break; + #elif defined(PH_0) + case 'H' : alt_all = GPIOH->AFR[ind]; break; + #elif defined(PI_0) + case 'I' : alt_all = GPIOI->AFR[ind]; break; + #elif defined(PJ_0) + case 'J' : alt_all = GPIOJ->AFR[ind]; break; + #elif defined(PK_0) + case 'K' : alt_all = GPIOK->AFR[ind]; break; + #elif defined(PL_0) + case 'L' : alt_all = GPIOL->AFR[ind]; break; + #endif + } + if (over_7) pin_number -= 8; + + uint8_t alt_func = (alt_all >> (4 * pin_number)) & 0x0F; + SERIAL_ECHOPGM("Alt Function: ", alt_func); + if (alt_func < 10) SERIAL_CHAR(' '); + SERIAL_ECHOPGM(" - "); + switch (alt_func) { + case 0 : SERIAL_ECHOPGM("system (misc. I/O)"); break; + case 1 : SERIAL_ECHOPGM("TIM1/TIM2 (probably PWM)"); break; + case 2 : SERIAL_ECHOPGM("TIM3..5 (probably PWM)"); break; + case 3 : SERIAL_ECHOPGM("TIM8..11 (probably PWM)"); break; + case 4 : SERIAL_ECHOPGM("I2C1..3"); break; + case 5 : SERIAL_ECHOPGM("SPI1/SPI2"); break; + case 6 : SERIAL_ECHOPGM("SPI3"); break; + case 7 : SERIAL_ECHOPGM("USART1..3"); break; + case 8 : SERIAL_ECHOPGM("USART4..6"); break; + case 9 : SERIAL_ECHOPGM("CAN1/CAN2, TIM12..14 (probably PWM)"); break; + case 10 : SERIAL_ECHOPGM("OTG"); break; + case 11 : SERIAL_ECHOPGM("ETH"); break; + case 12 : SERIAL_ECHOPGM("FSMC, SDIO, OTG"); break; + case 13 : SERIAL_ECHOPGM("DCMI"); break; + case 14 : SERIAL_ECHOPGM("unused (shouldn't see this)"); break; + case 15 : SERIAL_ECHOPGM("EVENTOUT"); break; + } + } + #else + // TODO: F1 doesn't support changing pins function, so we need to check the function of the PIN and if it's enabled + #endif +} // pwm_details diff --git a/src/HAL/STM32/pins_Xref.h b/src/HAL/STM32/pins_Xref.h new file mode 100644 index 0000000..890e561 --- /dev/null +++ b/src/HAL/STM32/pins_Xref.h @@ -0,0 +1,612 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +// +// make a list of the Arduino pin numbers in the Port/Pin order +// +#ifdef PA0 + PIN_ADD(PA0) +#endif +#ifdef PA1 + PIN_ADD(PA1) +#endif +#ifdef PA2 + PIN_ADD(PA2) +#endif +#ifdef PA3 + PIN_ADD(PA3) +#endif +#ifdef PA4 + PIN_ADD(PA4) +#endif +#ifdef PA5 + PIN_ADD(PA5) +#endif +#ifdef PA6 + PIN_ADD(PA6) +#endif +#ifdef PA7 + PIN_ADD(PA7) +#endif +#ifdef PA8 + PIN_ADD(PA8) +#endif +#ifdef PA9 + PIN_ADD(PA9) +#endif +#ifdef PA10 + PIN_ADD(PA10) +#endif +#ifdef PA11 + PIN_ADD(PA11) +#endif +#ifdef PA12 + PIN_ADD(PA12) +#endif +#ifdef PA13 + PIN_ADD(PA13) +#endif +#ifdef PA14 + PIN_ADD(PA14) +#endif +#ifdef PA15 + PIN_ADD(PA15) +#endif + +#ifdef PB0 + PIN_ADD(PB0) +#endif +#ifdef PB1 + PIN_ADD(PB1) +#endif +#ifdef PB2 + PIN_ADD(PB2) +#endif +#ifdef PB3 + PIN_ADD(PB3) +#endif +#ifdef PB4 + PIN_ADD(PB4) +#endif +#ifdef PB5 + PIN_ADD(PB5) +#endif +#ifdef PB6 + PIN_ADD(PB6) +#endif +#ifdef PB7 + PIN_ADD(PB7) +#endif +#ifdef PB8 + PIN_ADD(PB8) +#endif +#ifdef PB9 + PIN_ADD(PB9) +#endif +#ifdef PB10 + PIN_ADD(PB10) +#endif +#ifdef PB11 + PIN_ADD(PB11) +#endif +#ifdef PB12 + PIN_ADD(PB12) +#endif +#ifdef PB13 + PIN_ADD(PB13) +#endif +#ifdef PB14 + PIN_ADD(PB14) +#endif +#ifdef PB15 + PIN_ADD(PB15) +#endif + +#ifdef PC0 + PIN_ADD(PC0) +#endif +#ifdef PC1 + PIN_ADD(PC1) +#endif +#ifdef PC2 + PIN_ADD(PC2) +#endif +#ifdef PC3 + PIN_ADD(PC3) +#endif +#ifdef PC4 + PIN_ADD(PC4) +#endif +#ifdef PC5 + PIN_ADD(PC5) +#endif +#ifdef PC6 + PIN_ADD(PC6) +#endif +#ifdef PC7 + PIN_ADD(PC7) +#endif +#ifdef PC8 + PIN_ADD(PC8) +#endif +#ifdef PC9 + PIN_ADD(PC9) +#endif +#ifdef PC10 + PIN_ADD(PC10) +#endif +#ifdef PC11 + PIN_ADD(PC11) +#endif +#ifdef PC12 + PIN_ADD(PC12) +#endif +#ifdef PC13 + PIN_ADD(PC13) +#endif +#ifdef PC14 + PIN_ADD(PC14) +#endif +#ifdef PC15 + PIN_ADD(PC15) +#endif + +#ifdef PD0 + PIN_ADD(PD0) +#endif +#ifdef PD1 + PIN_ADD(PD1) +#endif +#ifdef PD2 + PIN_ADD(PD2) +#endif +#ifdef PD3 + PIN_ADD(PD3) +#endif +#ifdef PD4 + PIN_ADD(PD4) +#endif +#ifdef PD5 + PIN_ADD(PD5) +#endif +#ifdef PD6 + PIN_ADD(PD6) +#endif +#ifdef PD7 + PIN_ADD(PD7) +#endif +#ifdef PD8 + PIN_ADD(PD8) +#endif +#ifdef PD9 + PIN_ADD(PD9) +#endif +#ifdef PD10 + PIN_ADD(PD10) +#endif +#ifdef PD11 + PIN_ADD(PD11) +#endif +#ifdef PD12 + PIN_ADD(PD12) +#endif +#ifdef PD13 + PIN_ADD(PD13) +#endif +#ifdef PD14 + PIN_ADD(PD14) +#endif +#ifdef PD15 + PIN_ADD(PD15) +#endif + +#ifdef PE0 + PIN_ADD(PE0) +#endif +#ifdef PE1 + PIN_ADD(PE1) +#endif +#ifdef PE2 + PIN_ADD(PE2) +#endif +#ifdef PE3 + PIN_ADD(PE3) +#endif +#ifdef PE4 + PIN_ADD(PE4) +#endif +#ifdef PE5 + PIN_ADD(PE5) +#endif +#ifdef PE6 + PIN_ADD(PE6) +#endif +#ifdef PE7 + PIN_ADD(PE7) +#endif +#ifdef PE8 + PIN_ADD(PE8) +#endif +#ifdef PE9 + PIN_ADD(PE9) +#endif +#ifdef PE10 + PIN_ADD(PE10) +#endif +#ifdef PE11 + PIN_ADD(PE11) +#endif +#ifdef PE12 + PIN_ADD(PE12) +#endif +#ifdef PE13 + PIN_ADD(PE13) +#endif +#ifdef PE14 + PIN_ADD(PE14) +#endif +#ifdef PE15 + PIN_ADD(PE15) +#endif + +#ifdef PF0 + PIN_ADD(PF0) +#endif +#ifdef PF1 + PIN_ADD(PF1) +#endif +#ifdef PF2 + PIN_ADD(PF2) +#endif +#ifdef PF3 + PIN_ADD(PF3) +#endif +#ifdef PF4 + PIN_ADD(PF4) +#endif +#ifdef PF5 + PIN_ADD(PF5) +#endif +#ifdef PF6 + PIN_ADD(PF6) +#endif +#ifdef PF7 + PIN_ADD(PF7) +#endif +#ifdef PF8 + PIN_ADD(PF8) +#endif +#ifdef PF9 + PIN_ADD(PF9) +#endif +#ifdef PF10 + PIN_ADD(PF10) +#endif +#ifdef PF11 + PIN_ADD(PF11) +#endif +#ifdef PF12 + PIN_ADD(PF12) +#endif +#ifdef PF13 + PIN_ADD(PF13) +#endif +#ifdef PF14 + PIN_ADD(PF14) +#endif +#ifdef PF15 + PIN_ADD(PF15) +#endif + +#ifdef PG0 + PIN_ADD(PG0) +#endif +#ifdef PG1 + PIN_ADD(PG1) +#endif +#ifdef PG2 + PIN_ADD(PG2) +#endif +#ifdef PG3 + PIN_ADD(PG3) +#endif +#ifdef PG4 + PIN_ADD(PG4) +#endif +#ifdef PG5 + PIN_ADD(PG5) +#endif +#ifdef PG6 + PIN_ADD(PG6) +#endif +#ifdef PG7 + PIN_ADD(PG7) +#endif +#ifdef PG8 + PIN_ADD(PG8) +#endif +#ifdef PG9 + PIN_ADD(PG9) +#endif +#ifdef PG10 + PIN_ADD(PG10) +#endif +#ifdef PG11 + PIN_ADD(PG11) +#endif +#ifdef PG12 + PIN_ADD(PG12) +#endif +#ifdef PG13 + PIN_ADD(PG13) +#endif +#ifdef PG14 + PIN_ADD(PG14) +#endif +#ifdef PG15 + PIN_ADD(PG15) +#endif + +#ifdef PH0 + PIN_ADD(PH0) +#endif +#ifdef PH1 + PIN_ADD(PH1) +#endif +#ifdef PH2 + PIN_ADD(PH2) +#endif +#ifdef PH3 + PIN_ADD(PH3) +#endif +#ifdef PH4 + PIN_ADD(PH4) +#endif +#ifdef PH5 + PIN_ADD(PH5) +#endif +#ifdef PH6 + PIN_ADD(PH6) +#endif +#ifdef PH7 + PIN_ADD(PH7) +#endif +#ifdef PH8 + PIN_ADD(PH8) +#endif +#ifdef PH9 + PIN_ADD(PH9) +#endif +#ifdef PH10 + PIN_ADD(PH10) +#endif +#ifdef PH11 + PIN_ADD(PH11) +#endif +#ifdef PH12 + PIN_ADD(PH12) +#endif +#ifdef PH13 + PIN_ADD(PH13) +#endif +#ifdef PH14 + PIN_ADD(PH14) +#endif +#ifdef PH15 + PIN_ADD(PH15) +#endif + +#ifdef PI0 + PIN_ADD(PI0) +#endif +#ifdef PI1 + PIN_ADD(PI1) +#endif +#ifdef PI2 + PIN_ADD(PI2) +#endif +#ifdef PI3 + PIN_ADD(PI3) +#endif +#ifdef PI4 + PIN_ADD(PI4) +#endif +#ifdef PI5 + PIN_ADD(PI5) +#endif +#ifdef PI6 + PIN_ADD(PI6) +#endif +#ifdef PI7 + PIN_ADD(PI7) +#endif +#ifdef PI8 + PIN_ADD(PI8) +#endif +#ifdef PI9 + PIN_ADD(PI9) +#endif +#ifdef PI10 + PIN_ADD(PI10) +#endif +#ifdef PI11 + PIN_ADD(PI11) +#endif +#ifdef PI12 + PIN_ADD(PI12) +#endif +#ifdef PI13 + PIN_ADD(PI13) +#endif +#ifdef PI14 + PIN_ADD(PI14) +#endif +#ifdef PI15 + PIN_ADD(PI15) +#endif + +#ifdef PJ0 + PIN_ADD(PJ0) +#endif +#ifdef PJ1 + PIN_ADD(PJ1) +#endif +#ifdef PJ2 + PIN_ADD(PJ2) +#endif +#ifdef PJ3 + PIN_ADD(PJ3) +#endif +#ifdef PJ4 + PIN_ADD(PJ4) +#endif +#ifdef PJ5 + PIN_ADD(PJ5) +#endif +#ifdef PJ6 + PIN_ADD(PJ6) +#endif +#ifdef PJ7 + PIN_ADD(PJ7) +#endif +#ifdef PJ8 + PIN_ADD(PJ8) +#endif +#ifdef PJ9 + PIN_ADD(PJ9) +#endif +#ifdef PJ10 + PIN_ADD(PJ10) +#endif +#ifdef PJ11 + PIN_ADD(PJ11) +#endif +#ifdef PJ12 + PIN_ADD(PJ12) +#endif +#ifdef PJ13 + PIN_ADD(PJ13) +#endif +#ifdef PJ14 + PIN_ADD(PJ14) +#endif +#ifdef PJ15 + PIN_ADD(PJ15) +#endif + +#ifdef PK0 + PIN_ADD(PK0) +#endif +#ifdef PK1 + PIN_ADD(PK1) +#endif +#ifdef PK2 + PIN_ADD(PK2) +#endif +#ifdef PK3 + PIN_ADD(PK3) +#endif +#ifdef PK4 + PIN_ADD(PK4) +#endif +#ifdef PK5 + PIN_ADD(PK5) +#endif +#ifdef PK6 + PIN_ADD(PK6) +#endif +#ifdef PK7 + PIN_ADD(PK7) +#endif +#ifdef PK8 + PIN_ADD(PK8) +#endif +#ifdef PK9 + PIN_ADD(PK9) +#endif +#ifdef PK10 + PIN_ADD(PK10) +#endif +#ifdef PK11 + PIN_ADD(PK11) +#endif +#ifdef PK12 + PIN_ADD(PK12) +#endif +#ifdef PK13 + PIN_ADD(PK13) +#endif +#ifdef PK14 + PIN_ADD(PK14) +#endif +#ifdef PK15 + PIN_ADD(PK15) +#endif + +#ifdef PL0 + PIN_ADD(PL0) +#endif +#ifdef PL1 + PIN_ADD(PL1) +#endif +#ifdef PL2 + PIN_ADD(PL2) +#endif +#ifdef PL3 + PIN_ADD(PL3) +#endif +#ifdef PL4 + PIN_ADD(PL4) +#endif +#ifdef PL5 + PIN_ADD(PL5) +#endif +#ifdef PL6 + PIN_ADD(PL6) +#endif +#ifdef PL7 + PIN_ADD(PL7) +#endif +#ifdef PL8 + PIN_ADD(PL8) +#endif +#ifdef PL9 + PIN_ADD(PL9) +#endif +#ifdef PL10 + PIN_ADD(PL10) +#endif +#ifdef PL11 + PIN_ADD(PL11) +#endif +#ifdef PL12 + PIN_ADD(PL12) +#endif +#ifdef PL13 + PIN_ADD(PL13) +#endif +#ifdef PL14 + PIN_ADD(PL14) +#endif +#ifdef PL15 + PIN_ADD(PL15) +#endif diff --git a/src/HAL/STM32/sdio.cpp b/src/HAL/STM32/sdio.cpp new file mode 100644 index 0000000..41fe90b --- /dev/null +++ b/src/HAL/STM32/sdio.cpp @@ -0,0 +1,451 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SDIO_SUPPORT) + +#include "sdio.h" + +#include +#include + +#if defined(STM32F103xE) || defined(STM32F103xG) + #include + #include +#elif defined(STM32F4xx) + #include + #include + #include + #include +#elif defined(STM32F7xx) + #include + #include + #include + #include +#elif defined(STM32H7xx) + #define SDIO_FOR_STM32H7 + #include + #include + #include + #include +#else + #error "SDIO is only supported with STM32F103xE, STM32F103xG, STM32F4xx, STM32F7xx, and STM32H7xx." +#endif + +// SDIO Max Clock (naming from STM Manual, don't change) +#define SDIOCLK 48000000 + +// Target Clock, configurable. Default is 18MHz, from STM32F1 +#ifndef SDIO_CLOCK + #define SDIO_CLOCK 18000000 // 18 MHz +#endif + +SD_HandleTypeDef hsd; // SDIO structure + +static uint32_t clock_to_divider(uint32_t clk) { + #ifdef SDIO_FOR_STM32H7 + // SDMMC_CK frequency = sdmmc_ker_ck / [2 * CLKDIV]. + uint32_t sdmmc_clk = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_SDMMC); + return sdmmc_clk / (2U * SDIO_CLOCK) + (sdmmc_clk % (2U * SDIO_CLOCK) != 0); + #else + // limit the SDIO master clock to 8/3 of PCLK2. See STM32 Manuals + // Also limited to no more than 48Mhz (SDIOCLK). + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + clk = min(clk, (uint32_t)(pclk2 * 8 / 3)); + clk = min(clk, (uint32_t)SDIOCLK); + // Round up divider, so we don't run the card over the speed supported, + // and subtract by 2, because STM32 will add 2, as written in the manual: + // SDIO_CK frequency = SDIOCLK / [CLKDIV + 2] + return pclk2 / clk + (pclk2 % clk != 0) - 2; + #endif +} + +// Start the SDIO clock +void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { + UNUSED(hsd); + #ifdef SDIO_FOR_STM32H7 + pinmap_pinout(PC_12, PinMap_SD); + pinmap_pinout(PD_2, PinMap_SD); + pinmap_pinout(PC_8, PinMap_SD); + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // Define D1-D3 only for 4-bit wide SDIO bus + pinmap_pinout(PC_9, PinMap_SD); + pinmap_pinout(PC_10, PinMap_SD); + pinmap_pinout(PC_11, PinMap_SD); + #endif + __HAL_RCC_SDMMC1_CLK_ENABLE(); + HAL_NVIC_EnableIRQ(SDMMC1_IRQn); + #else + __HAL_RCC_SDIO_CLK_ENABLE(); + #endif +} + +#ifdef SDIO_FOR_STM32H7 + + #define SD_TIMEOUT 1000 // ms + + extern "C" void SDMMC1_IRQHandler(void) { HAL_SD_IRQHandler(&hsd); } + + uint8_t waitingRxCplt = 0, waitingTxCplt = 0; + void HAL_SD_TxCpltCallback(SD_HandleTypeDef *hsdio) { waitingTxCplt = 0; } + void HAL_SD_RxCpltCallback(SD_HandleTypeDef *hsdio) { waitingRxCplt = 0; } + + void HAL_SD_MspDeInit(SD_HandleTypeDef *hsd) { + __HAL_RCC_SDMMC1_FORCE_RESET(); delay(10); + __HAL_RCC_SDMMC1_RELEASE_RESET(); delay(10); + } + + bool SDIO_Init() { + HAL_StatusTypeDef sd_state = HAL_OK; + if (hsd.Instance == SDMMC1) HAL_SD_DeInit(&hsd); + + // HAL SD initialization + hsd.Instance = SDMMC1; + hsd.Init.ClockEdge = SDMMC_CLOCK_EDGE_RISING; + hsd.Init.ClockPowerSave = SDMMC_CLOCK_POWER_SAVE_DISABLE; + hsd.Init.BusWide = SDMMC_BUS_WIDE_1B; + hsd.Init.HardwareFlowControl = SDMMC_HARDWARE_FLOW_CONTROL_DISABLE; + hsd.Init.ClockDiv = clock_to_divider(SDIO_CLOCK); + sd_state = HAL_SD_Init(&hsd); + + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) + if (sd_state == HAL_OK) + sd_state = HAL_SD_ConfigWideBusOperation(&hsd, SDMMC_BUS_WIDE_4B); + #endif + + return (sd_state == HAL_OK); + } + +#else // !SDIO_FOR_STM32H7 + + #define SD_TIMEOUT 500 // ms + + // SDIO retries, configurable. Default is 3, from STM32F1 + #ifndef SDIO_READ_RETRIES + #define SDIO_READ_RETRIES 3 + #endif + + // F4 supports one DMA for RX and another for TX, but Marlin will never + // do read and write at same time, so we use the same DMA for both. + DMA_HandleTypeDef hdma_sdio; + + #ifdef STM32F1xx + #define DMA_IRQ_HANDLER DMA2_Channel4_5_IRQHandler + #elif defined(STM32F4xx) + #define DMA_IRQ_HANDLER DMA2_Stream3_IRQHandler + #else + #error "Unknown STM32 architecture." + #endif + + extern "C" void SDIO_IRQHandler(void) { HAL_SD_IRQHandler(&hsd); } + extern "C" void DMA_IRQ_HANDLER(void) { HAL_DMA_IRQHandler(&hdma_sdio); } + + /* + SDIO_INIT_CLK_DIV is 118 + SDIO clock frequency is 48MHz / (TRANSFER_CLOCK_DIV + 2) + SDIO init clock frequency should not exceed 400kHz = 48MHz / (118 + 2) + + Default TRANSFER_CLOCK_DIV is 2 (118 / 40) + Default SDIO clock frequency is 48MHz / (2 + 2) = 12 MHz + This might be too fast for stable SDIO operations + + MKS Robin SDIO seems stable with BusWide 1bit and ClockDiv 8 (i.e., 4.8MHz SDIO clock frequency) + More testing is required as there are clearly some 4bit init problems. + */ + + void go_to_transfer_speed() { + /* Default SDIO peripheral configuration for SD card initialization */ + hsd.Init.ClockEdge = hsd.Init.ClockEdge; + hsd.Init.ClockBypass = hsd.Init.ClockBypass; + hsd.Init.ClockPowerSave = hsd.Init.ClockPowerSave; + hsd.Init.BusWide = hsd.Init.BusWide; + hsd.Init.HardwareFlowControl = hsd.Init.HardwareFlowControl; + hsd.Init.ClockDiv = clock_to_divider(SDIO_CLOCK); + + /* Initialize SDIO peripheral interface with default configuration */ + SDIO_Init(hsd.Instance, hsd.Init); + } + + void SD_LowLevel_Init() { + uint32_t tempreg; + + // Enable GPIO clocks + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + + GPIO_InitTypeDef GPIO_InitStruct; + + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = 1; // GPIO_NOPULL + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + + #if DISABLED(STM32F1xx) + GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; + #endif + + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_12; // D0 & SCK + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // define D1-D3 only if have a four bit wide SDIO bus + GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11; // D1-D3 + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + #endif + + // Configure PD.02 CMD line + GPIO_InitStruct.Pin = GPIO_PIN_2; + HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + + // Setup DMA + #ifdef STM32F1xx + hdma_sdio.Init.Mode = DMA_NORMAL; + hdma_sdio.Instance = DMA2_Channel4; + HAL_NVIC_EnableIRQ(DMA2_Channel4_5_IRQn); + #elif defined(STM32F4xx) + hdma_sdio.Init.Mode = DMA_PFCTRL; + hdma_sdio.Instance = DMA2_Stream3; + hdma_sdio.Init.Channel = DMA_CHANNEL_4; + hdma_sdio.Init.FIFOMode = DMA_FIFOMODE_ENABLE; + hdma_sdio.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + hdma_sdio.Init.MemBurst = DMA_MBURST_INC4; + hdma_sdio.Init.PeriphBurst = DMA_PBURST_INC4; + HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); + #endif + HAL_NVIC_EnableIRQ(SDIO_IRQn); + hdma_sdio.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_sdio.Init.MemInc = DMA_MINC_ENABLE; + hdma_sdio.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + hdma_sdio.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + hdma_sdio.Init.Priority = DMA_PRIORITY_LOW; + __HAL_LINKDMA(&hsd, hdmarx, hdma_sdio); + __HAL_LINKDMA(&hsd, hdmatx, hdma_sdio); + + #ifdef STM32F1xx + __HAL_RCC_SDIO_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + #else + __HAL_RCC_SDIO_FORCE_RESET(); delay(2); + __HAL_RCC_SDIO_RELEASE_RESET(); delay(2); + __HAL_RCC_SDIO_CLK_ENABLE(); + + __HAL_RCC_DMA2_FORCE_RESET(); delay(2); + __HAL_RCC_DMA2_RELEASE_RESET(); delay(2); + __HAL_RCC_DMA2_CLK_ENABLE(); + #endif + + // Initialize the SDIO (with initial <400Khz Clock) + tempreg = 0 // Reset value + | SDIO_CLKCR_CLKEN // Clock enabled + | SDIO_INIT_CLK_DIV; // Clock Divider. Clock = 48000 / (118 + 2) = 400Khz + // Keep the rest at 0 => HW_Flow Disabled, Rising Clock Edge, Disable CLK ByPass, Bus Width = 0, Power save Disable + SDIO->CLKCR = tempreg; + + // Power up the SDIO + SDIO_PowerState_ON(SDIO); + hsd.Instance = SDIO; + } + + bool SDIO_Init() { + uint8_t retryCnt = SDIO_READ_RETRIES; + + bool status; + hsd.Instance = SDIO; + hsd.State = HAL_SD_STATE_RESET; + + SD_LowLevel_Init(); + + uint8_t retry_Cnt = retryCnt; + for (;;) { + hal.watchdog_refresh(); + status = (bool) HAL_SD_Init(&hsd); + if (!status) break; + if (!--retry_Cnt) return false; // return failing status if retries are exhausted + } + + go_to_transfer_speed(); + + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // go to 4 bit wide mode if pins are defined + retry_Cnt = retryCnt; + for (;;) { + hal.watchdog_refresh(); + if (!HAL_SD_ConfigWideBusOperation(&hsd, SDIO_BUS_WIDE_4B)) break; // some cards are only 1 bit wide so a pass here is not required + if (!--retry_Cnt) break; + } + if (!retry_Cnt) { // wide bus failed, go back to one bit wide mode + hsd.State = (HAL_SD_StateTypeDef) 0; // HAL_SD_STATE_RESET + SD_LowLevel_Init(); + retry_Cnt = retryCnt; + for (;;) { + hal.watchdog_refresh(); + status = (bool) HAL_SD_Init(&hsd); + if (!status) break; + if (!--retry_Cnt) return false; // return failing status if retries are exhausted + } + go_to_transfer_speed(); + } + #endif + + return true; + } + + /** + * @brief Read or Write a block + * @details Read or Write a block with SDIO + * + * @param block The block index + * @param src The data buffer source for a write + * @param dst The data buffer destination for a read + * + * @return true on success + */ + static bool SDIO_ReadWriteBlock_DMA(uint32_t block, const uint8_t *src, uint8_t *dst) { + if (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) return false; + + hal.watchdog_refresh(); + + HAL_StatusTypeDef ret; + if (src) { + hdma_sdio.Init.Direction = DMA_MEMORY_TO_PERIPH; + HAL_DMA_Init(&hdma_sdio); + ret = HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t*)src, block, 1); + } + else { + hdma_sdio.Init.Direction = DMA_PERIPH_TO_MEMORY; + HAL_DMA_Init(&hdma_sdio); + ret = HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t*)dst, block, 1); + } + + if (ret != HAL_OK) { + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + return false; + } + + millis_t timeout = millis() + SD_TIMEOUT; + // Wait the transfer + while (hsd.State != HAL_SD_STATE_READY) { + if (ELAPSED(millis(), timeout)) { + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + return false; + } + } + + while (__HAL_DMA_GET_FLAG(&hdma_sdio, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_sdio)) != 0 + || __HAL_DMA_GET_FLAG(&hdma_sdio, __HAL_DMA_GET_TE_FLAG_INDEX(&hdma_sdio)) != 0) { /* nada */ } + + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + + timeout = millis() + SD_TIMEOUT; + while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) if (ELAPSED(millis(), timeout)) return false; + + return true; + } + +#endif // !SDIO_FOR_STM32H7 + +/** + * @brief Read a block + * @details Read a block from media with SDIO + * + * @param block The block index + * @param src The block buffer + * + * @return true on success + */ +bool SDIO_ReadBlock(uint32_t block, uint8_t *dst) { + #ifdef SDIO_FOR_STM32H7 + + uint32_t timeout = HAL_GetTick() + SD_TIMEOUT; + + while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) + if (HAL_GetTick() >= timeout) return false; + + waitingRxCplt = 1; + if (HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t*)dst, block, 1) != HAL_OK) + return false; + + timeout = HAL_GetTick() + SD_TIMEOUT; + while (waitingRxCplt) + if (HAL_GetTick() >= timeout) return false; + + return true; + + #else + + uint8_t retries = SDIO_READ_RETRIES; + while (retries--) if (SDIO_ReadWriteBlock_DMA(block, nullptr, dst)) return true; + return false; + + #endif +} + +/** + * @brief Write a block + * @details Write a block to media with SDIO + * + * @param block The block index + * @param src The block data + * + * @return true on success + */ +bool SDIO_WriteBlock(uint32_t block, const uint8_t *src) { + #ifdef SDIO_FOR_STM32H7 + + uint32_t timeout = HAL_GetTick() + SD_TIMEOUT; + + while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) + if (HAL_GetTick() >= timeout) return false; + + waitingTxCplt = 1; + if (HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t*)src, block, 1) != HAL_OK) + return false; + + timeout = HAL_GetTick() + SD_TIMEOUT; + while (waitingTxCplt) + if (HAL_GetTick() >= timeout) return false; + + return true; + + #else + + uint8_t retries = SDIO_READ_RETRIES; + while (retries--) if (SDIO_ReadWriteBlock_DMA(block, src, nullptr)) return true; + return false; + + #endif +} + +bool SDIO_IsReady() { + return hsd.State == HAL_SD_STATE_READY; +} + +uint32_t SDIO_GetCardSize() { + return (uint32_t)(hsd.SdCard.BlockNbr) * (hsd.SdCard.BlockSize); +} + +#endif // SDIO_SUPPORT +#endif // HAL_STM32 diff --git a/src/HAL/STM32/sdio.h b/src/HAL/STM32/sdio.h new file mode 100644 index 0000000..cf5539c --- /dev/null +++ b/src/HAL/STM32/sdio.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define SDIO_D0_PIN PC8 +#define SDIO_D1_PIN PC9 +#define SDIO_D2_PIN PC10 +#define SDIO_D3_PIN PC11 +#define SDIO_CK_PIN PC12 +#define SDIO_CMD_PIN PD2 diff --git a/src/HAL/STM32/spi_pins.h b/src/HAL/STM32/spi_pins.h new file mode 100644 index 0000000..7f341a8 --- /dev/null +++ b/src/HAL/STM32/spi_pins.h @@ -0,0 +1,38 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Define SPI Pins: SCK, MISO, MOSI, SS + */ +#ifndef SD_SCK_PIN + #define SD_SCK_PIN PIN_SPI_SCK +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN PIN_SPI_MISO +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN PIN_SPI_MOSI +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN PIN_SPI_SS +#endif diff --git a/src/HAL/STM32/tft/gt911.cpp b/src/HAL/STM32/tft/gt911.cpp new file mode 100644 index 0000000..180abc6 --- /dev/null +++ b/src/HAL/STM32/tft/gt911.cpp @@ -0,0 +1,208 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TFT_TOUCH_DEVICE_GT911) + +#include "gt911.h" +#include "pinconfig.h" + +SW_IIC::SW_IIC(uint16_t sda, uint16_t scl) { + scl_pin = scl; + sda_pin = sda; +} + +// Software I2C hardware io init +void SW_IIC::init() { + OUT_WRITE(scl_pin, HIGH); + OUT_WRITE(sda_pin, HIGH); +} + +// Software I2C start signal +void SW_IIC::start() { + write_sda(HIGH); // SDA = 1 + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_sda(LOW); // SDA = 0 + iic_delay(1); + write_scl(LOW); // SCL = 0 // keep SCL low, avoid false stop caused by level jump caused by SDA switching IN/OUT +} + +// Software I2C stop signal +void SW_IIC::stop() { + write_scl(LOW); // SCL = 0 + iic_delay(2); + write_sda(LOW); // SDA = 0 + iic_delay(2); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_sda(HIGH); // SDA = 1 +} + +// Software I2C sends ACK or NACK signal +void SW_IIC::send_ack(bool ack) { + write_sda(ack ? LOW : HIGH); // SDA = !ack + iic_delay(2); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_scl(LOW); // SCL = 0 +} + +// Software I2C read ACK or NACK signal +bool SW_IIC::read_ack() { + bool error = 0; + set_sda_in(); + + iic_delay(2); + + write_scl(HIGH); // SCL = 1 + error = read_sda(); + + iic_delay(2); + + write_scl(LOW); // SCL = 0 + + set_sda_out(); + return error; +} + +void SW_IIC::send_byte(uint8_t txd) { + LOOP_L_N(i, 8) { + write_sda(txd & 0x80); // write data bit + txd <<= 1; + iic_delay(1); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_scl(LOW); // SCL = 0 + iic_delay(1); + } + + read_ack(); // wait ack +} + +uint8_t SW_IIC::read_byte(bool ack) { + uint8_t data = 0; + + set_sda_in(); + LOOP_L_N(i, 8) { + write_scl(HIGH); // SCL = 1 + iic_delay(1); + data <<= 1; + if (read_sda()) data++; + write_scl(LOW); // SCL = 0 + iic_delay(2); + } + set_sda_out(); + + send_ack(ack); + + return data; +} + +GT911_REG_MAP GT911::reg; +SW_IIC GT911::sw_iic = SW_IIC(GT911_SW_I2C_SDA_PIN, GT911_SW_I2C_SCL_PIN); + +void GT911::write_reg(uint16_t reg, uint8_t reg_len, uint8_t* w_data, uint8_t w_len) { + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address); // Set IIC Slave address + LOOP_L_N(i, reg_len) { // Set reg address + uint8_t r = (reg >> (8 * (reg_len - 1 - i))) & 0xFF; + sw_iic.send_byte(r); + } + + LOOP_L_N(i, w_len) { // Write data to reg + sw_iic.send_byte(w_data[i]); + } + sw_iic.stop(); +} + +void GT911::read_reg(uint16_t reg, uint8_t reg_len, uint8_t* r_data, uint8_t r_len) { + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address); // Set IIC Slave address + LOOP_L_N(i, reg_len) { // Set reg address + uint8_t r = (reg >> (8 * (reg_len - 1 - i))) & 0xFF; + sw_iic.send_byte(r); + } + + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address + 1); // Set read mode + + LOOP_L_N(i, r_len) { + r_data[i] = sw_iic.read_byte(1); // Read data from reg + } + sw_iic.stop(); +} + +void GT911::Init() { + OUT_WRITE(GT911_RST_PIN, LOW); + OUT_WRITE(GT911_INT_PIN, LOW); + delay(11); + WRITE(GT911_INT_PIN, HIGH); + delayMicroseconds(110); + WRITE(GT911_RST_PIN, HIGH); + delay(6); + WRITE(GT911_INT_PIN, LOW); + delay(55); + SET_INPUT(GT911_INT_PIN); + + sw_iic.init(); + + uint8_t clear_reg = 0x00; + write_reg(0x814E, 2, &clear_reg, 1); // Reset to 0 for start +} + +bool GT911::getFirstTouchPoint(int16_t *x, int16_t *y) { + read_reg(0x814E, 2, ®.REG.status, 1); + + if (reg.REG.status >= 0x80 && reg.REG.status <= 0x85) { + read_reg(0x8150, 2, reg.map + 2, 38); + uint8_t clear_reg = 0x00; + write_reg(0x814E, 2, &clear_reg, 1); // Reset to 0 for start + // First touch point + *x = ((reg.REG.point[0].xh & 0x0F) << 8) | reg.REG.point[0].xl; + *y = ((reg.REG.point[0].yh & 0x0F) << 8) | reg.REG.point[0].yl; + return true; + } + return false; +} + +bool GT911::getPoint(int16_t *x, int16_t *y) { + static bool touched = 0; + static int16_t read_x = 0, read_y = 0; + static millis_t next_time = 0; + + if (ELAPSED(millis(), next_time)) { + touched = getFirstTouchPoint(&read_x, &read_y); + next_time = millis() + 20; + } + + *x = read_x; + *y = read_y; + return touched; +} + +#endif // TFT_TOUCH_DEVICE_GT911 +#endif // HAL_STM32 diff --git a/src/HAL/STM32/tft/gt911.h b/src/HAL/STM32/tft/gt911.h new file mode 100644 index 0000000..6ecfe8b --- /dev/null +++ b/src/HAL/STM32/tft/gt911.h @@ -0,0 +1,120 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#define GT911_SLAVE_ADDRESS 0x28 + +#if !PIN_EXISTS(GT911_RST) + #error "GT911_RST_PIN is not defined." +#elif !PIN_EXISTS(GT911_INT) + #error "GT911_INT_PIN is not defined." +#elif !PIN_EXISTS(GT911_SW_I2C_SCL) + #error "GT911_SW_I2C_SCL_PIN is not defined." +#elif !PIN_EXISTS(GT911_SW_I2C_SDA) + #error "GT911_SW_I2C_SDA_PIN is not defined." +#endif + +class SW_IIC { + private: + uint16_t scl_pin; + uint16_t sda_pin; + void write_scl(bool level) + { + WRITE(scl_pin, level); + } + void write_sda(bool level) + { + WRITE(sda_pin, level); + } + bool read_sda() + { + return READ(sda_pin); + } + void set_sda_out() + { + SET_OUTPUT(sda_pin); + } + void set_sda_in() + { + SET_INPUT_PULLUP(sda_pin); + } + static void iic_delay(uint8_t t) + { + delayMicroseconds(t); + } + + public: + SW_IIC(uint16_t sda, uint16_t scl); + // setSCL/SDA have to be called before begin() + void setSCL(uint16_t scl) + { + scl_pin = scl; + }; + void setSDA(uint16_t sda) + { + sda_pin = sda; + }; + void init(); // Initialize the IO port of IIC + void start(); // Send IIC start signal + void stop(); // Send IIC stop signal + void send_byte(uint8_t txd); // IIC sends a byte + uint8_t read_byte(bool ack); // IIC reads a byte + void send_ack(bool ack); // IIC sends ACK or NACK signal + bool read_ack(); +}; + +typedef struct __attribute__((__packed__)) { + uint8_t xl; + uint8_t xh; + uint8_t yl; + uint8_t yh; + uint8_t sizel; + uint8_t sizeh; + uint8_t reserved; + uint8_t track_id; +} GT911_POINT; + +typedef union __attribute__((__packed__)) { + uint8_t map[42]; + struct { + uint8_t status; // 0x814E + uint8_t track_id; // 0x814F + + GT911_POINT point[5]; // [0]:0x8150 - 0x8157 / [1]:0x8158 - 0x815F / [2]:0x8160 - 0x8167 / [3]:0x8168 - 0x816F / [4]:0x8170 - 0x8177 + } REG; +} GT911_REG_MAP; + +class GT911 { + private: + static const uint8_t gt911_slave_address = GT911_SLAVE_ADDRESS; + static GT911_REG_MAP reg; + static SW_IIC sw_iic; + static void write_reg(uint16_t reg, uint8_t reg_len, uint8_t* w_data, uint8_t w_len); + static void read_reg(uint16_t reg, uint8_t reg_len, uint8_t* r_data, uint8_t r_len); + + public: + static void Init(); + static bool getFirstTouchPoint(int16_t *x, int16_t *y); + static bool getPoint(int16_t *x, int16_t *y); +}; diff --git a/src/HAL/STM32/tft/tft_fsmc.cpp b/src/HAL/STM32/tft/tft_fsmc.cpp new file mode 100644 index 0000000..e68b3c1 --- /dev/null +++ b/src/HAL/STM32/tft/tft_fsmc.cpp @@ -0,0 +1,174 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if HAS_FSMC_TFT + +#include "tft_fsmc.h" +#include "pinconfig.h" + +SRAM_HandleTypeDef TFT_FSMC::SRAMx; +DMA_HandleTypeDef TFT_FSMC::DMAtx; +LCD_CONTROLLER_TypeDef *TFT_FSMC::LCD; + +void TFT_FSMC::Init() { + uint32_t controllerAddress; + FSMC_NORSRAM_TimingTypeDef Timing, ExtTiming; + + uint32_t NSBank = (uint32_t)pinmap_peripheral(digitalPinToPinName(TFT_CS_PIN), PinMap_FSMC_CS); + + // Perform the SRAM1 memory initialization sequence + SRAMx.Instance = FSMC_NORSRAM_DEVICE; + SRAMx.Extended = FSMC_NORSRAM_EXTENDED_DEVICE; + // SRAMx.Init + SRAMx.Init.NSBank = NSBank; + SRAMx.Init.DataAddressMux = FSMC_DATA_ADDRESS_MUX_DISABLE; + SRAMx.Init.MemoryType = FSMC_MEMORY_TYPE_SRAM; + SRAMx.Init.MemoryDataWidth = TERN(TFT_INTERFACE_FSMC_8BIT, FSMC_NORSRAM_MEM_BUS_WIDTH_8, FSMC_NORSRAM_MEM_BUS_WIDTH_16); + SRAMx.Init.BurstAccessMode = FSMC_BURST_ACCESS_MODE_DISABLE; + SRAMx.Init.WaitSignalPolarity = FSMC_WAIT_SIGNAL_POLARITY_LOW; + SRAMx.Init.WrapMode = FSMC_WRAP_MODE_DISABLE; + SRAMx.Init.WaitSignalActive = FSMC_WAIT_TIMING_BEFORE_WS; + SRAMx.Init.WriteOperation = FSMC_WRITE_OPERATION_ENABLE; + SRAMx.Init.WaitSignal = FSMC_WAIT_SIGNAL_DISABLE; + SRAMx.Init.ExtendedMode = FSMC_EXTENDED_MODE_ENABLE; + SRAMx.Init.AsynchronousWait = FSMC_ASYNCHRONOUS_WAIT_DISABLE; + SRAMx.Init.WriteBurst = FSMC_WRITE_BURST_DISABLE; + #ifdef STM32F4xx + SRAMx.Init.PageSize = FSMC_PAGE_SIZE_NONE; + #endif + // Read Timing - relatively slow to ensure ID information is correctly read from TFT controller + // Can be decreases from 15-15-24 to 4-4-8 with risk of stability loss + Timing.AddressSetupTime = 15; + Timing.AddressHoldTime = 15; + Timing.DataSetupTime = 24; + Timing.BusTurnAroundDuration = 0; + Timing.CLKDivision = 16; + Timing.DataLatency = 17; + Timing.AccessMode = FSMC_ACCESS_MODE_A; + // Write Timing + // Can be decreases from 8-15-8 to 0-0-1 with risk of stability loss + ExtTiming.AddressSetupTime = 8; + ExtTiming.AddressHoldTime = 15; + ExtTiming.DataSetupTime = 8; + ExtTiming.BusTurnAroundDuration = 0; + ExtTiming.CLKDivision = 16; + ExtTiming.DataLatency = 17; + ExtTiming.AccessMode = FSMC_ACCESS_MODE_A; + + __HAL_RCC_FSMC_CLK_ENABLE(); + + for (uint16_t i = 0; PinMap_FSMC[i].pin != NC; i++) + pinmap_pinout(PinMap_FSMC[i].pin, PinMap_FSMC); + pinmap_pinout(digitalPinToPinName(TFT_CS_PIN), PinMap_FSMC_CS); + pinmap_pinout(digitalPinToPinName(TFT_RS_PIN), PinMap_FSMC_RS); + + controllerAddress = FSMC_BANK1_1; + #ifdef PF0 + switch (NSBank) { + case FSMC_NORSRAM_BANK2: controllerAddress = FSMC_BANK1_2 ; break; + case FSMC_NORSRAM_BANK3: controllerAddress = FSMC_BANK1_3 ; break; + case FSMC_NORSRAM_BANK4: controllerAddress = FSMC_BANK1_4 ; break; + } + #endif + + controllerAddress |= (uint32_t)pinmap_peripheral(digitalPinToPinName(TFT_RS_PIN), PinMap_FSMC_RS); + + HAL_SRAM_Init(&SRAMx, &Timing, &ExtTiming); + + __HAL_RCC_DMA2_CLK_ENABLE(); + + #ifdef STM32F1xx + DMAtx.Instance = DMA2_Channel1; + #elif defined(STM32F4xx) + DMAtx.Instance = DMA2_Stream0; + DMAtx.Init.Channel = DMA_CHANNEL_0; + DMAtx.Init.FIFOMode = DMA_FIFOMODE_ENABLE; + DMAtx.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + DMAtx.Init.MemBurst = DMA_MBURST_SINGLE; + DMAtx.Init.PeriphBurst = DMA_PBURST_SINGLE; + #endif + + DMAtx.Init.Direction = DMA_MEMORY_TO_MEMORY; + DMAtx.Init.MemInc = DMA_MINC_DISABLE; + DMAtx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + DMAtx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + DMAtx.Init.Mode = DMA_NORMAL; + DMAtx.Init.Priority = DMA_PRIORITY_HIGH; + + LCD = (LCD_CONTROLLER_TypeDef *)controllerAddress; +} + +uint32_t TFT_FSMC::GetID() { + uint32_t id; + WriteReg(0); + id = LCD->RAM; + + if (id == 0) + id = ReadID(LCD_READ_ID); + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = ReadID(LCD_READ_ID4); + return id; +} + +uint32_t TFT_FSMC::ReadID(tft_data_t Reg) { + uint32_t id; + WriteReg(Reg); + id = LCD->RAM; // dummy read + id = Reg << 24; + id |= (LCD->RAM & 0x00FF) << 16; + id |= (LCD->RAM & 0x00FF) << 8; + id |= LCD->RAM & 0x00FF; + return id; +} + +bool TFT_FSMC::isBusy() { + #if defined(STM32F1xx) + volatile bool dmaEnabled = (DMAtx.Instance->CCR & DMA_CCR_EN) != RESET; + #elif defined(STM32F4xx) + volatile bool dmaEnabled = DMAtx.Instance->CR & DMA_SxCR_EN; + #endif + if (dmaEnabled) { + if (__HAL_DMA_GET_FLAG(&DMAtx, __HAL_DMA_GET_TC_FLAG_INDEX(&DMAtx)) != 0 || __HAL_DMA_GET_FLAG(&DMAtx, __HAL_DMA_GET_TE_FLAG_INDEX(&DMAtx)) != 0) + Abort(); + } + else + Abort(); + return dmaEnabled; +} + +void TFT_FSMC::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + DMAtx.Init.PeriphInc = MemoryIncrease; + HAL_DMA_Init(&DMAtx); + DataTransferBegin(); + HAL_DMA_Start(&DMAtx, (uint32_t)Data, (uint32_t)&(LCD->RAM), Count); + HAL_DMA_PollForTransfer(&DMAtx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); + Abort(); +} + +#endif // HAS_FSMC_TFT +#endif // HAL_STM32 diff --git a/src/HAL/STM32/tft/tft_fsmc.h b/src/HAL/STM32/tft/tft_fsmc.h new file mode 100644 index 0000000..2200aba --- /dev/null +++ b/src/HAL/STM32/tft/tft_fsmc.h @@ -0,0 +1,171 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#ifdef STM32F1xx + #include "stm32f1xx_hal.h" +#elif defined(STM32F4xx) + #include "stm32f4xx_hal.h" +#else + #error "FSMC TFT is currently only supported on STM32F1 and STM32F4 hardware." +#endif + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT SPI_DATASIZE_8BIT +#define DATASIZE_16BIT SPI_DATASIZE_16BIT +#define TFT_IO_DRIVER TFT_FSMC + +#define TFT_DATASIZE TERN(TFT_INTERFACE_FSMC_8BIT, DATASIZE_8BIT, DATASIZE_16BIT) +typedef TERN(TFT_INTERFACE_FSMC_8BIT, uint8_t, uint16_t) tft_data_t; + +typedef struct { + __IO tft_data_t REG; + __IO tft_data_t RAM; +} LCD_CONTROLLER_TypeDef; + +class TFT_FSMC { + private: + static SRAM_HandleTypeDef SRAMx; + static DMA_HandleTypeDef DMAtx; + + static LCD_CONTROLLER_TypeDef *LCD; + + static uint32_t ReadID(tft_data_t Reg); + static void Transmit(tft_data_t Data) { LCD->RAM = Data; __DSB(); } + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + + public: + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort() { __HAL_DMA_DISABLE(&DMAtx); } + + static void DataTransferBegin(uint16_t DataWidth = TFT_DATASIZE) {} + static void DataTransferEnd() {}; + + static void WriteData(uint16_t Data) { Transmit(tft_data_t(Data)); } + static void WriteReg(uint16_t Reg) { LCD->REG = tft_data_t(Reg); __DSB(); } + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_PINC_ENABLE, Data, Count); } + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_PINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; + +#ifdef STM32F1xx + #define FSMC_PIN_DATA STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, AFIO_NONE) +#elif defined(STM32F4xx) + #define FSMC_PIN_DATA STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF12_FSMC) + #define FSMC_BANK1_1 0x60000000U + #define FSMC_BANK1_2 0x64000000U + #define FSMC_BANK1_3 0x68000000U + #define FSMC_BANK1_4 0x6C000000U +#else + #error No configuration for this MCU +#endif + +const PinMap PinMap_FSMC[] = { + {PD_14, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D00 + {PD_15, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D01 + {PD_0, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D02 + {PD_1, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D03 + {PE_7, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D04 + {PE_8, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D05 + {PE_9, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D06 + {PE_10, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D07 + #if DISABLED(TFT_INTERFACE_FSMC_8BIT) + {PE_11, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D08 + {PE_12, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D09 + {PE_13, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D10 + {PE_14, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D11 + {PE_15, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D12 + {PD_8, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D13 + {PD_9, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D14 + {PD_10, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_D15 + #endif + {PD_4, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_NOE + {PD_5, FSMC_NORSRAM_DEVICE, FSMC_PIN_DATA}, // FSMC_NWE + {NC, NP, 0} +}; + +const PinMap PinMap_FSMC_CS[] = { + {PD_7, (void *)FSMC_NORSRAM_BANK1, FSMC_PIN_DATA}, // FSMC_NE1 + #ifdef PF0 + {PG_9, (void *)FSMC_NORSRAM_BANK2, FSMC_PIN_DATA}, // FSMC_NE2 + {PG_10, (void *)FSMC_NORSRAM_BANK3, FSMC_PIN_DATA}, // FSMC_NE3 + {PG_12, (void *)FSMC_NORSRAM_BANK4, FSMC_PIN_DATA}, // FSMC_NE4 + #endif + {NC, NP, 0} +}; + +#if ENABLED(TFT_INTERFACE_FSMC_8BIT) + #define FSMC_RS(A) (void *)((2 << (A-1)) - 1) +#else + #define FSMC_RS(A) (void *)((2 << A) - 2) +#endif + +const PinMap PinMap_FSMC_RS[] = { + #ifdef PF0 + {PF_0, FSMC_RS( 0), FSMC_PIN_DATA}, // FSMC_A0 + {PF_1, FSMC_RS( 1), FSMC_PIN_DATA}, // FSMC_A1 + {PF_2, FSMC_RS( 2), FSMC_PIN_DATA}, // FSMC_A2 + {PF_3, FSMC_RS( 3), FSMC_PIN_DATA}, // FSMC_A3 + {PF_4, FSMC_RS( 4), FSMC_PIN_DATA}, // FSMC_A4 + {PF_5, FSMC_RS( 5), FSMC_PIN_DATA}, // FSMC_A5 + {PF_12, FSMC_RS( 6), FSMC_PIN_DATA}, // FSMC_A6 + {PF_13, FSMC_RS( 7), FSMC_PIN_DATA}, // FSMC_A7 + {PF_14, FSMC_RS( 8), FSMC_PIN_DATA}, // FSMC_A8 + {PF_15, FSMC_RS( 9), FSMC_PIN_DATA}, // FSMC_A9 + {PG_0, FSMC_RS(10), FSMC_PIN_DATA}, // FSMC_A10 + {PG_1, FSMC_RS(11), FSMC_PIN_DATA}, // FSMC_A11 + {PG_2, FSMC_RS(12), FSMC_PIN_DATA}, // FSMC_A12 + {PG_3, FSMC_RS(13), FSMC_PIN_DATA}, // FSMC_A13 + {PG_4, FSMC_RS(14), FSMC_PIN_DATA}, // FSMC_A14 + {PG_5, FSMC_RS(15), FSMC_PIN_DATA}, // FSMC_A15 + #endif + {PD_11, FSMC_RS(16), FSMC_PIN_DATA}, // FSMC_A16 + {PD_12, FSMC_RS(17), FSMC_PIN_DATA}, // FSMC_A17 + {PD_13, FSMC_RS(18), FSMC_PIN_DATA}, // FSMC_A18 + {PE_3, FSMC_RS(19), FSMC_PIN_DATA}, // FSMC_A19 + {PE_4, FSMC_RS(20), FSMC_PIN_DATA}, // FSMC_A20 + {PE_5, FSMC_RS(21), FSMC_PIN_DATA}, // FSMC_A21 + {PE_6, FSMC_RS(22), FSMC_PIN_DATA}, // FSMC_A22 + {PE_2, FSMC_RS(23), FSMC_PIN_DATA}, // FSMC_A23 + #ifdef PF0 + {PG_13, FSMC_RS(24), FSMC_PIN_DATA}, // FSMC_A24 + {PG_14, FSMC_RS(25), FSMC_PIN_DATA}, // FSMC_A25 + #endif + {NC, NP, 0} +}; diff --git a/src/HAL/STM32/tft/tft_ltdc.cpp b/src/HAL/STM32/tft/tft_ltdc.cpp new file mode 100644 index 0000000..95871bf --- /dev/null +++ b/src/HAL/STM32/tft/tft_ltdc.cpp @@ -0,0 +1,389 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if HAS_LTDC_TFT + +#include "tft_ltdc.h" +#include "pinconfig.h" + +#define FRAME_BUFFER_ADDRESS 0XC0000000 // SDRAM address + +#define SDRAM_TIMEOUT ((uint32_t)0xFFFF) +#define REFRESH_COUNT ((uint32_t)0x02A5) // SDRAM refresh counter + +#define SDRAM_MODEREG_BURST_LENGTH_1 ((uint16_t)0x0000) +#define SDRAM_MODEREG_BURST_LENGTH_2 ((uint16_t)0x0001) +#define SDRAM_MODEREG_BURST_LENGTH_4 ((uint16_t)0x0002) +#define SDRAM_MODEREG_BURST_LENGTH_8 ((uint16_t)0x0004) +#define SDRAM_MODEREG_BURST_TYPE_SEQUENTIAL ((uint16_t)0x0000) +#define SDRAM_MODEREG_BURST_TYPE_INTERLEAVED ((uint16_t)0x0008) +#define SDRAM_MODEREG_CAS_LATENCY_2 ((uint16_t)0x0020) +#define SDRAM_MODEREG_CAS_LATENCY_3 ((uint16_t)0x0030) +#define SDRAM_MODEREG_OPERATING_MODE_STANDARD ((uint16_t)0x0000) +#define SDRAM_MODEREG_WRITEBURST_MODE_PROGRAMMED ((uint16_t)0x0000) +#define SDRAM_MODEREG_WRITEBURST_MODE_SINGLE ((uint16_t)0x0200) + +void SDRAM_Initialization_Sequence(SDRAM_HandleTypeDef *hsdram, FMC_SDRAM_CommandTypeDef *Command) { + + __IO uint32_t tmpmrd =0; + /* Step 1: Configure a clock configuration enable command */ + Command->CommandMode = FMC_SDRAM_CMD_CLK_ENABLE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 2: Insert 100 us minimum delay */ + /* Inserted delay is equal to 1 ms due to systick time base unit (ms) */ + HAL_Delay(1); + + /* Step 3: Configure a PALL (precharge all) command */ + Command->CommandMode = FMC_SDRAM_CMD_PALL; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 4 : Configure a Auto-Refresh command */ + Command->CommandMode = FMC_SDRAM_CMD_AUTOREFRESH_MODE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 8; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 5: Program the external memory mode register */ + tmpmrd = (uint32_t)(SDRAM_MODEREG_BURST_LENGTH_1 | + SDRAM_MODEREG_BURST_TYPE_SEQUENTIAL | + SDRAM_MODEREG_CAS_LATENCY_2 | + SDRAM_MODEREG_OPERATING_MODE_STANDARD | + SDRAM_MODEREG_WRITEBURST_MODE_SINGLE); + + Command->CommandMode = FMC_SDRAM_CMD_LOAD_MODE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = tmpmrd; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 6: Set the refresh rate counter */ + /* Set the device refresh rate */ + HAL_SDRAM_ProgramRefreshRate(hsdram, REFRESH_COUNT); +} + +void SDRAM_Config() { + + __HAL_RCC_SYSCFG_CLK_ENABLE(); + __HAL_RCC_FMC_CLK_ENABLE(); + + SDRAM_HandleTypeDef hsdram; + FMC_SDRAM_TimingTypeDef SDRAM_Timing; + FMC_SDRAM_CommandTypeDef command; + + /* Configure the SDRAM device */ + hsdram.Instance = FMC_SDRAM_DEVICE; + hsdram.Init.SDBank = FMC_SDRAM_BANK1; + hsdram.Init.ColumnBitsNumber = FMC_SDRAM_COLUMN_BITS_NUM_9; + hsdram.Init.RowBitsNumber = FMC_SDRAM_ROW_BITS_NUM_13; + hsdram.Init.MemoryDataWidth = FMC_SDRAM_MEM_BUS_WIDTH_16; + hsdram.Init.InternalBankNumber = FMC_SDRAM_INTERN_BANKS_NUM_4; + hsdram.Init.CASLatency = FMC_SDRAM_CAS_LATENCY_2; + hsdram.Init.WriteProtection = FMC_SDRAM_WRITE_PROTECTION_DISABLE; + hsdram.Init.SDClockPeriod = FMC_SDRAM_CLOCK_PERIOD_2; + hsdram.Init.ReadBurst = FMC_SDRAM_RBURST_ENABLE; + hsdram.Init.ReadPipeDelay = FMC_SDRAM_RPIPE_DELAY_0; + + /* Timing configuration for 100Mhz as SDRAM clock frequency (System clock is up to 200Mhz) */ + SDRAM_Timing.LoadToActiveDelay = 2; + SDRAM_Timing.ExitSelfRefreshDelay = 8; + SDRAM_Timing.SelfRefreshTime = 6; + SDRAM_Timing.RowCycleDelay = 6; + SDRAM_Timing.WriteRecoveryTime = 2; + SDRAM_Timing.RPDelay = 2; + SDRAM_Timing.RCDDelay = 2; + + /* Initialize the SDRAM controller */ + if (HAL_SDRAM_Init(&hsdram, &SDRAM_Timing) != HAL_OK) + { + /* Initialization Error */ + } + + /* Program the SDRAM external device */ + SDRAM_Initialization_Sequence(&hsdram, &command); +} + +void LTDC_Config() { + + __HAL_RCC_LTDC_CLK_ENABLE(); + __HAL_RCC_DMA2D_CLK_ENABLE(); + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct; + + /* The PLL3R is configured to provide the LTDC PCLK clock */ + /* PLL3_VCO Input = HSE_VALUE / PLL3M = 25Mhz / 5 = 5 Mhz */ + /* PLL3_VCO Output = PLL3_VCO Input * PLL3N = 5Mhz * 160 = 800 Mhz */ + /* PLLLCDCLK = PLL3_VCO Output/PLL3R = 800Mhz / 16 = 50Mhz */ + /* LTDC clock frequency = PLLLCDCLK = 50 Mhz */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_LTDC; + PeriphClkInitStruct.PLL3.PLL3M = 5; + PeriphClkInitStruct.PLL3.PLL3N = 160; + PeriphClkInitStruct.PLL3.PLL3FRACN = 0; + PeriphClkInitStruct.PLL3.PLL3P = 2; + PeriphClkInitStruct.PLL3.PLL3Q = 2; + PeriphClkInitStruct.PLL3.PLL3R = (800 / LTDC_LCD_CLK); + PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOWIDE; + PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_2; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); + + LTDC_HandleTypeDef hltdc_F; + LTDC_LayerCfgTypeDef pLayerCfg; + + /* LTDC Initialization -------------------------------------------------------*/ + + /* Polarity configuration */ + /* Initialize the horizontal synchronization polarity as active low */ + hltdc_F.Init.HSPolarity = LTDC_HSPOLARITY_AL; + /* Initialize the vertical synchronization polarity as active low */ + hltdc_F.Init.VSPolarity = LTDC_VSPOLARITY_AL; + /* Initialize the data enable polarity as active low */ + hltdc_F.Init.DEPolarity = LTDC_DEPOLARITY_AL; + /* Initialize the pixel clock polarity as input pixel clock */ + hltdc_F.Init.PCPolarity = LTDC_PCPOLARITY_IPC; + + /* Timing configuration */ + hltdc_F.Init.HorizontalSync = (LTDC_LCD_HSYNC - 1); + hltdc_F.Init.VerticalSync = (LTDC_LCD_VSYNC - 1); + hltdc_F.Init.AccumulatedHBP = (LTDC_LCD_HSYNC + LTDC_LCD_HBP - 1); + hltdc_F.Init.AccumulatedVBP = (LTDC_LCD_VSYNC + LTDC_LCD_VBP - 1); + hltdc_F.Init.AccumulatedActiveH = (TFT_HEIGHT + LTDC_LCD_VSYNC + LTDC_LCD_VBP - 1); + hltdc_F.Init.AccumulatedActiveW = (TFT_WIDTH + LTDC_LCD_HSYNC + LTDC_LCD_HBP - 1); + hltdc_F.Init.TotalHeigh = (TFT_HEIGHT + LTDC_LCD_VSYNC + LTDC_LCD_VBP + LTDC_LCD_VFP - 1); + hltdc_F.Init.TotalWidth = (TFT_WIDTH + LTDC_LCD_HSYNC + LTDC_LCD_HBP + LTDC_LCD_HFP - 1); + + /* Configure R,G,B component values for LCD background color : all black background */ + hltdc_F.Init.Backcolor.Blue = 0; + hltdc_F.Init.Backcolor.Green = 0; + hltdc_F.Init.Backcolor.Red = 0; + + hltdc_F.Instance = LTDC; + + /* Layer0 Configuration ------------------------------------------------------*/ + + /* Windowing configuration */ + pLayerCfg.WindowX0 = 0; + pLayerCfg.WindowX1 = TFT_WIDTH; + pLayerCfg.WindowY0 = 0; + pLayerCfg.WindowY1 = TFT_HEIGHT; + + /* Pixel Format configuration*/ + pLayerCfg.PixelFormat = LTDC_PIXEL_FORMAT_RGB565; + + /* Start Address configuration : frame buffer is located at SDRAM memory */ + pLayerCfg.FBStartAdress = (uint32_t)(FRAME_BUFFER_ADDRESS); + + /* Alpha constant (255 == totally opaque) */ + pLayerCfg.Alpha = 255; + + /* Default Color configuration (configure A,R,G,B component values) : no background color */ + pLayerCfg.Alpha0 = 0; /* fully transparent */ + pLayerCfg.Backcolor.Blue = 0; + pLayerCfg.Backcolor.Green = 0; + pLayerCfg.Backcolor.Red = 0; + + /* Configure blending factors */ + pLayerCfg.BlendingFactor1 = LTDC_BLENDING_FACTOR1_CA; + pLayerCfg.BlendingFactor2 = LTDC_BLENDING_FACTOR2_CA; + + /* Configure the number of lines and number of pixels per line */ + pLayerCfg.ImageWidth = TFT_WIDTH; + pLayerCfg.ImageHeight = TFT_HEIGHT; + + /* Configure the LTDC */ + if (HAL_LTDC_Init(&hltdc_F) != HAL_OK) + { + /* Initialization Error */ + } + + /* Configure the Layer*/ + if (HAL_LTDC_ConfigLayer(&hltdc_F, &pLayerCfg, 0) != HAL_OK) + { + /* Initialization Error */ + } +} + +uint16_t TFT_LTDC::x_min = 0; +uint16_t TFT_LTDC::x_max = 0; +uint16_t TFT_LTDC::y_min = 0; +uint16_t TFT_LTDC::y_max = 0; +uint16_t TFT_LTDC::x_cur = 0; +uint16_t TFT_LTDC::y_cur = 0; +uint8_t TFT_LTDC::reg = 0; +volatile uint16_t* TFT_LTDC::framebuffer = (volatile uint16_t* )FRAME_BUFFER_ADDRESS; + +void TFT_LTDC::Init() { + + // SDRAM pins init + for (uint16_t i = 0; PinMap_SDRAM[i].pin != NC; i++) + pinmap_pinout(PinMap_SDRAM[i].pin, PinMap_SDRAM); + + // SDRAM peripheral config + SDRAM_Config(); + + // LTDC pins init + for (uint16_t i = 0; PinMap_LTDC[i].pin != NC; i++) + pinmap_pinout(PinMap_LTDC[i].pin, PinMap_LTDC); + + // LTDC peripheral config + LTDC_Config(); +} + +uint32_t TFT_LTDC::GetID() { + return 0xABAB; +} + +uint32_t TFT_LTDC::ReadID(tft_data_t Reg) { + return 0xABAB; +} + +bool TFT_LTDC::isBusy() { + return false; +} + +uint16_t TFT_LTDC::ReadPoint(uint16_t x, uint16_t y) { + return framebuffer[(TFT_WIDTH * y) + x]; +} + +void TFT_LTDC::DrawPoint(uint16_t x, uint16_t y, uint16_t color) { + framebuffer[(TFT_WIDTH * y) + x] = color; +} + +void TFT_LTDC::DrawRect(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t color) { + + if (sx == ex || sy == ey) return; + + uint16_t offline = TFT_WIDTH - (ex - sx); + uint32_t addr = (uint32_t)&framebuffer[(TFT_WIDTH * sy) + sx]; + + CBI(DMA2D->CR, 0); + DMA2D->CR = 3 << 16; + DMA2D->OPFCCR = 0X02; + DMA2D->OOR = offline; + DMA2D->OMAR = addr; + DMA2D->NLR = (ey - sy) | ((ex - sx) << 16); + DMA2D->OCOLR = color; + SBI(DMA2D->CR, 0); + + uint32_t timeout = 0; + while (!TEST(DMA2D->ISR, 1)) { + timeout++; + if (timeout > 0x1FFFFF) break; + } + SBI(DMA2D->IFCR, 1); +} + +void TFT_LTDC::DrawImage(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t *colors) { + + if (sx == ex || sy == ey) return; + + uint16_t offline = TFT_WIDTH - (ex - sx); + uint32_t addr = (uint32_t)&framebuffer[(TFT_WIDTH * sy) + sx]; + + CBI(DMA2D->CR, 0); + DMA2D->CR = 0 << 16; + DMA2D->FGPFCCR = 0X02; + DMA2D->FGOR = 0; + DMA2D->OOR = offline; + DMA2D->FGMAR = (uint32_t)colors; + DMA2D->OMAR = addr; + DMA2D->NLR = (ey - sy) | ((ex - sx) << 16); + SBI(DMA2D->CR, 0); + + uint32_t timeout = 0; + while (!TEST(DMA2D->ISR, 1)) { + timeout++; + if (timeout > 0x1FFFFF) break; + } + SBI(DMA2D->IFCR, 1); +} + +void TFT_LTDC::WriteData(uint16_t data) { + switch (reg) { + case 0x01: x_cur = x_min = data; return; + case 0x02: x_max = data; return; + case 0x03: y_cur = y_min = data; return; + case 0x04: y_max = data; return; + } + Transmit(data); +} + +void TFT_LTDC::Transmit(tft_data_t Data) { + DrawPoint(x_cur, y_cur, Data); + x_cur++; + if (x_cur > x_max) { + x_cur = x_min; + y_cur++; + if (y_cur > y_max) y_cur = y_min; + } +} + +void TFT_LTDC::WriteReg(uint16_t Reg) { + reg = Reg; +} + +void TFT_LTDC::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + + while (x_cur != x_min && Count) { + Transmit(*Data); + if (MemoryIncrease == DMA_PINC_ENABLE) Data++; + Count--; + } + + uint16_t width = x_max - x_min + 1; + uint16_t height = Count / width; + uint16_t x_end_cnt = Count - (width * height); + + if (height) { + if (MemoryIncrease == DMA_PINC_ENABLE) { + DrawImage(x_min, y_cur, x_min + width, y_cur + height, Data); + Data += width * height; + } + else + DrawRect(x_min, y_cur, x_min + width, y_cur + height, *Data); + y_cur += height; + } + + while (x_end_cnt) { + Transmit(*Data); + if (MemoryIncrease == DMA_PINC_ENABLE) Data++; + x_end_cnt--; + } +} + +#endif // HAS_LTDC_TFT +#endif // HAL_STM32 diff --git a/src/HAL/STM32/tft/tft_ltdc.h b/src/HAL/STM32/tft/tft_ltdc.h new file mode 100644 index 0000000..7b63d69 --- /dev/null +++ b/src/HAL/STM32/tft/tft_ltdc.h @@ -0,0 +1,155 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#ifdef STM32H7xx + #include "stm32h7xx_hal.h" +#else + #error "LTDC TFT is currently only supported on STM32H7 hardware." +#endif + +#define DATASIZE_8BIT SPI_DATASIZE_8BIT +#define DATASIZE_16BIT SPI_DATASIZE_16BIT +#define TFT_IO_DRIVER TFT_LTDC + +#define TFT_DATASIZE DATASIZE_16BIT +typedef uint16_t tft_data_t; + +class TFT_LTDC { + private: + static volatile uint16_t *framebuffer; + static uint16_t x_min, x_max, y_min, y_max, x_cur, y_cur; + static uint8_t reg; + + static uint32_t ReadID(tft_data_t Reg); + + static uint16_t ReadPoint(uint16_t x, uint16_t y); + static void DrawPoint(uint16_t x, uint16_t y, uint16_t color); + static void DrawRect(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t color); + static void DrawImage(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t *colors); + static void Transmit(tft_data_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + + public: + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort() { /*__HAL_DMA_DISABLE(&DMAtx);*/ } + + static void DataTransferBegin(uint16_t DataWidth = TFT_DATASIZE) {} + static void DataTransferEnd() {}; + + static void WriteData(uint16_t Data); + static void WriteReg(uint16_t Reg); + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_PINC_ENABLE, Data, Count); } + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_PINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; + +const PinMap PinMap_LTDC[] = { + {PF_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_DE + {PG_7, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_CLK + {PI_9, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_VSYNC + {PI_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_HSYNC + + {PG_6, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R7 + {PH_12, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R6 + {PH_11, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R5 + {PH_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R4 + {PH_9, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R3 + + {PI_2, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G7 + {PI_1, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G6 + {PI_0, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G5 + {PH_15, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G4 + {PH_14, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G3 + {PH_13, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G2 + + {PI_7, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B7 + {PI_6, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B6 + {PI_5, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B5 + {PI_4, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B4 + {PG_11, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B3 + {NC, NP, 0} +}; + +const PinMap PinMap_SDRAM[] = { + {PC_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNWE + {PC_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNE0 + {PC_3, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDCKE0 + {PE_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_NBL0 + {PE_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_NBL1 + {PF_11, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNRAS + {PG_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDCLK + {PG_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNCAS + {PG_4, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_BA0 + {PG_5, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_BA1 + {PD_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D0 + {PD_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D1 + {PD_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D2 + {PD_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D3 + {PE_7, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D4 + {PE_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D5 + {PE_9, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D6 + {PE_10, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D7 + {PE_11, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D8 + {PE_12, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D9 + {PE_13, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D10 + {PE_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D11 + {PE_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D12 + {PD_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D13 + {PD_9, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D14 + {PD_10, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D15 + {PF_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A0 + {PF_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A1 + {PF_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A2 + {PF_3, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A3 + {PF_4, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A4 + {PF_5, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A5 + {PF_12, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A6 + {PF_13, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A7 + {PF_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A8 + {PF_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A9 + {PG_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A10 + {PG_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A11 + {PG_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A12 + {NC, NP, 0} +}; + +const PinMap PinMap_QUADSPI[] = { + {PB_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_CLK + {PB_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_NCS + {PF_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PF_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO2 + {PF_8, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO0 + {PF_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO1 + {NC, NP, 0} +}; diff --git a/src/HAL/STM32/tft/tft_spi.cpp b/src/HAL/STM32/tft/tft_spi.cpp new file mode 100644 index 0000000..2e18c8a --- /dev/null +++ b/src/HAL/STM32/tft/tft_spi.cpp @@ -0,0 +1,270 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if HAS_SPI_TFT + +#include "tft_spi.h" +#include "pinconfig.h" + +SPI_HandleTypeDef TFT_SPI::SPIx; +DMA_HandleTypeDef TFT_SPI::DMAtx; + +void TFT_SPI::Init() { + SPI_TypeDef *spiInstance; + + OUT_WRITE(TFT_A0_PIN, HIGH); + OUT_WRITE(TFT_CS_PIN, HIGH); + + if ((spiInstance = (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TFT_SCK_PIN), PinMap_SPI_SCLK)) == NP) return; + if (spiInstance != (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TFT_MOSI_PIN), PinMap_SPI_MOSI)) return; + + #if PIN_EXISTS(TFT_MISO) && TFT_MISO_PIN != TFT_MOSI_PIN + if (spiInstance != (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TFT_MISO_PIN), PinMap_SPI_MISO)) return; + #endif + + SPIx.Instance = spiInstance; + SPIx.State = HAL_SPI_STATE_RESET; + SPIx.Init.NSS = SPI_NSS_SOFT; + SPIx.Init.Mode = SPI_MODE_MASTER; + SPIx.Init.Direction = (TFT_MISO_PIN == TFT_MOSI_PIN) ? SPI_DIRECTION_1LINE : SPI_DIRECTION_2LINES; + SPIx.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; + SPIx.Init.CLKPhase = SPI_PHASE_1EDGE; + SPIx.Init.CLKPolarity = SPI_POLARITY_LOW; + SPIx.Init.DataSize = SPI_DATASIZE_8BIT; + SPIx.Init.FirstBit = SPI_FIRSTBIT_MSB; + SPIx.Init.TIMode = SPI_TIMODE_DISABLE; + SPIx.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + SPIx.Init.CRCPolynomial = 10; + + pinmap_pinout(digitalPinToPinName(TFT_SCK_PIN), PinMap_SPI_SCLK); + pinmap_pinout(digitalPinToPinName(TFT_MOSI_PIN), PinMap_SPI_MOSI); + #if PIN_EXISTS(TFT_MISO) && TFT_MISO_PIN != TFT_MOSI_PIN + pinmap_pinout(digitalPinToPinName(TFT_MISO_PIN), PinMap_SPI_MISO); + #endif + pin_PullConfig(get_GPIO_Port(STM_PORT(digitalPinToPinName(TFT_SCK_PIN))), STM_LL_GPIO_PIN(digitalPinToPinName(TFT_SCK_PIN)), GPIO_PULLDOWN); + + #ifdef SPI1_BASE + if (SPIx.Instance == SPI1) { + __HAL_RCC_SPI1_CLK_ENABLE(); + #ifdef STM32F1xx + __HAL_RCC_DMA1_CLK_ENABLE(); + DMAtx.Instance = DMA1_Channel3; + #elif defined(STM32F4xx) + __HAL_RCC_DMA2_CLK_ENABLE(); + DMAtx.Instance = DMA2_Stream3; + DMAtx.Init.Channel = DMA_CHANNEL_3; + #endif + SPIx.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4; + } + #endif + #ifdef SPI2_BASE + if (SPIx.Instance == SPI2) { + __HAL_RCC_SPI2_CLK_ENABLE(); + #ifdef STM32F1xx + __HAL_RCC_DMA1_CLK_ENABLE(); + DMAtx.Instance = DMA1_Channel5; + #elif defined(STM32F4xx) + __HAL_RCC_DMA1_CLK_ENABLE(); + DMAtx.Instance = DMA1_Stream4; + DMAtx.Init.Channel = DMA_CHANNEL_0; + #endif + } + #endif + #ifdef SPI3_BASE + if (SPIx.Instance == SPI3) { + __HAL_RCC_SPI3_CLK_ENABLE(); + #ifdef STM32F1xx + __HAL_RCC_DMA2_CLK_ENABLE(); + DMAtx.Instance = DMA2_Channel2; + #elif defined(STM32F4xx) + __HAL_RCC_DMA1_CLK_ENABLE(); + DMAtx.Instance = DMA1_Stream5; + DMAtx.Init.Channel = DMA_CHANNEL_0; + #endif + } + #endif + + HAL_SPI_Init(&SPIx); + + DMAtx.Init.Direction = DMA_MEMORY_TO_PERIPH; + DMAtx.Init.PeriphInc = DMA_PINC_DISABLE; + DMAtx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + DMAtx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + DMAtx.Init.Mode = DMA_NORMAL; + DMAtx.Init.Priority = DMA_PRIORITY_LOW; + #ifdef STM32F4xx + DMAtx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + #endif +} + +void TFT_SPI::DataTransferBegin(uint16_t DataSize) { + SPIx.Init.DataSize = DataSize == DATASIZE_8BIT ? SPI_DATASIZE_8BIT : SPI_DATASIZE_16BIT; + HAL_SPI_Init(&SPIx); + WRITE(TFT_CS_PIN, LOW); +} + +#ifdef TFT_DEFAULT_DRIVER + #include "../../../lcd/tft_io/tft_ids.h" +#endif + +uint32_t TFT_SPI::GetID() { + uint32_t id; + id = ReadID(LCD_READ_ID); + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) { + id = ReadID(LCD_READ_ID4); + #ifdef TFT_DEFAULT_DRIVER + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = TFT_DEFAULT_DRIVER; + #endif + } + return id; +} + +uint32_t TFT_SPI::ReadID(uint16_t Reg) { + uint32_t Data = 0; + #if PIN_EXISTS(TFT_MISO) + uint32_t BaudRatePrescaler = SPIx.Init.BaudRatePrescaler; + uint32_t i; + + SPIx.Init.BaudRatePrescaler = SPIx.Instance == SPI1 ? SPI_BAUDRATEPRESCALER_8 : SPI_BAUDRATEPRESCALER_4; + DataTransferBegin(DATASIZE_8BIT); + WriteReg(Reg); + + if (SPIx.Init.Direction == SPI_DIRECTION_1LINE) SPI_1LINE_RX(&SPIx); + __HAL_SPI_ENABLE(&SPIx); + + for (i = 0; i < 4; i++) { + #if TFT_MISO_PIN != TFT_MOSI_PIN + //if (hspi->Init.Direction == SPI_DIRECTION_2LINES) { + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} + SPIx.Instance->DR = 0; + //} + #endif + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_RXNE)) {} + Data = (Data << 8) | SPIx.Instance->DR; + } + + __HAL_SPI_DISABLE(&SPIx); + DataTransferEnd(); + + SPIx.Init.BaudRatePrescaler = BaudRatePrescaler; + #endif + + return Data >> 7; +} + +bool TFT_SPI::isBusy() { + #if defined(STM32F1xx) + volatile bool dmaEnabled = (DMAtx.Instance->CCR & DMA_CCR_EN) != RESET; + #elif defined(STM32F4xx) + volatile bool dmaEnabled = DMAtx.Instance->CR & DMA_SxCR_EN; + #endif + if (dmaEnabled) { + if (__HAL_DMA_GET_FLAG(&DMAtx, __HAL_DMA_GET_TC_FLAG_INDEX(&DMAtx)) != 0 || __HAL_DMA_GET_FLAG(&DMAtx, __HAL_DMA_GET_TE_FLAG_INDEX(&DMAtx)) != 0) + Abort(); + } + else + Abort(); + return dmaEnabled; +} + +void TFT_SPI::Abort() { + // Wait for any running spi + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} + while ( __HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_BSY)) {} + // First, abort any running dma + HAL_DMA_Abort(&DMAtx); + // DeInit objects + HAL_DMA_DeInit(&DMAtx); + HAL_SPI_DeInit(&SPIx); + // Deselect CS + DataTransferEnd(); +} + +void TFT_SPI::Transmit(uint16_t Data) { + if (TFT_MISO_PIN == TFT_MOSI_PIN) + SPI_1LINE_TX(&SPIx); + + __HAL_SPI_ENABLE(&SPIx); + + SPIx.Instance->DR = Data; + + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} + while ( __HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_BSY)) {} + + if (TFT_MISO_PIN != TFT_MOSI_PIN) + __HAL_SPI_CLEAR_OVRFLAG(&SPIx); // Clear overrun flag in 2 Lines communication mode because received is not read +} + +void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + // Wait last dma finish, to start another + while (isBusy()) { /* nada */ } + + DMAtx.Init.MemInc = MemoryIncrease; + HAL_DMA_Init(&DMAtx); + + if (TFT_MISO_PIN == TFT_MOSI_PIN) + SPI_1LINE_TX(&SPIx); + + DataTransferBegin(); + + HAL_DMA_Start(&DMAtx, (uint32_t)Data, (uint32_t)&(SPIx.Instance->DR), Count); + __HAL_SPI_ENABLE(&SPIx); + + SET_BIT(SPIx.Instance->CR2, SPI_CR2_TXDMAEN); // Enable Tx DMA Request + + HAL_DMA_PollForTransfer(&DMAtx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); + Abort(); +} + +#if ENABLED(USE_SPI_DMA_TC) + + void TFT_SPI::TransmitDMA_IT(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + + DMAtx.Init.MemInc = MemoryIncrease; + HAL_DMA_Init(&DMAtx); + + if (TFT_MISO_PIN == TFT_MOSI_PIN) + SPI_1LINE_TX(&SPIx); + + DataTransferBegin(); + + HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); + HAL_DMA_Start_IT(&DMAtx, (uint32_t)Data, (uint32_t)&(SPIx.Instance->DR), Count); + __HAL_SPI_ENABLE(&SPIx); + + SET_BIT(SPIx.Instance->CR2, SPI_CR2_TXDMAEN); // Enable Tx DMA Request + } + + extern "C" void DMA2_Stream3_IRQHandler(void) { HAL_DMA_IRQHandler(&TFT_SPI::DMAtx); } + +#endif + +#endif // HAS_SPI_TFT +#endif // HAL_STM32 diff --git a/src/HAL/STM32/tft/tft_spi.h b/src/HAL/STM32/tft/tft_spi.h new file mode 100644 index 0000000..de051e2 --- /dev/null +++ b/src/HAL/STM32/tft/tft_spi.h @@ -0,0 +1,84 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifdef STM32F1xx + #include "stm32f1xx_hal.h" +#elif defined(STM32F4xx) + #include "stm32f4xx_hal.h" +#else + #error SPI TFT is currently only supported on STM32F1 and STM32F4 hardware. +#endif + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT SPI_DATASIZE_8BIT +#define DATASIZE_16BIT SPI_DATASIZE_16BIT +#define TFT_IO_DRIVER TFT_SPI + +class TFT_SPI { +private: + static SPI_HandleTypeDef SPIx; + + + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + #if ENABLED(USE_SPI_DMA_TC) + static void TransmitDMA_IT(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + #endif + +public: + static DMA_HandleTypeDef DMAtx; + + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATASIZE_16BIT); + static void DataTransferEnd() { WRITE(TFT_CS_PIN, HIGH); }; + static void DataTransferAbort(); + + static void WriteData(uint16_t Data) { Transmit(Data); } + static void WriteReg(uint16_t Reg) { WRITE(TFT_A0_PIN, LOW); Transmit(Reg); WRITE(TFT_A0_PIN, HIGH); } + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_MINC_ENABLE, Data, Count); } + + #if ENABLED(USE_SPI_DMA_TC) + static void WriteSequenceIT(uint16_t *Data, uint16_t Count) { TransmitDMA_IT(DMA_MINC_ENABLE, Data, Count); } + #endif + + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_MINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; diff --git a/src/HAL/STM32/tft/xpt2046.cpp b/src/HAL/STM32/tft/xpt2046.cpp new file mode 100644 index 0000000..cf4a8f1 --- /dev/null +++ b/src/HAL/STM32/tft/xpt2046.cpp @@ -0,0 +1,173 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS + +#include "xpt2046.h" +#include "pinconfig.h" + +uint16_t delta(uint16_t a, uint16_t b) { return a > b ? a - b : b - a; } + +SPI_HandleTypeDef XPT2046::SPIx; + +void XPT2046::Init() { + SPI_TypeDef *spiInstance; + + OUT_WRITE(TOUCH_CS_PIN, HIGH); + + #if PIN_EXISTS(TOUCH_INT) + // Optional Pendrive interrupt pin + SET_INPUT(TOUCH_INT_PIN); + #endif + + spiInstance = (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TOUCH_SCK_PIN), PinMap_SPI_SCLK); + if (spiInstance != (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TOUCH_MOSI_PIN), PinMap_SPI_MOSI)) spiInstance = NP; + if (spiInstance != (SPI_TypeDef *)pinmap_peripheral(digitalPinToPinName(TOUCH_MISO_PIN), PinMap_SPI_MISO)) spiInstance = NP; + + SPIx.Instance = spiInstance; + + if (SPIx.Instance) { + SPIx.State = HAL_SPI_STATE_RESET; + SPIx.Init.NSS = SPI_NSS_SOFT; + SPIx.Init.Mode = SPI_MODE_MASTER; + SPIx.Init.Direction = SPI_DIRECTION_2LINES; + SPIx.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8; + SPIx.Init.CLKPhase = SPI_PHASE_2EDGE; + SPIx.Init.CLKPolarity = SPI_POLARITY_HIGH; + SPIx.Init.DataSize = SPI_DATASIZE_8BIT; + SPIx.Init.FirstBit = SPI_FIRSTBIT_MSB; + SPIx.Init.TIMode = SPI_TIMODE_DISABLE; + SPIx.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + SPIx.Init.CRCPolynomial = 10; + + pinmap_pinout(digitalPinToPinName(TOUCH_SCK_PIN), PinMap_SPI_SCLK); + pinmap_pinout(digitalPinToPinName(TOUCH_MOSI_PIN), PinMap_SPI_MOSI); + pinmap_pinout(digitalPinToPinName(TOUCH_MISO_PIN), PinMap_SPI_MISO); + + #ifdef SPI1_BASE + if (SPIx.Instance == SPI1) { + __HAL_RCC_SPI1_CLK_ENABLE(); + SPIx.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16; + } + #endif + #ifdef SPI2_BASE + if (SPIx.Instance == SPI2) { + __HAL_RCC_SPI2_CLK_ENABLE(); + } + #endif + #ifdef SPI3_BASE + if (SPIx.Instance == SPI3) { + __HAL_RCC_SPI3_CLK_ENABLE(); + } + #endif + } + else { + SPIx.Instance = nullptr; + SET_INPUT(TOUCH_MISO_PIN); + SET_OUTPUT(TOUCH_MOSI_PIN); + SET_OUTPUT(TOUCH_SCK_PIN); + } + + getRawData(XPT2046_Z1); +} + +bool XPT2046::isTouched() { + return isBusy() ? false : ( + #if PIN_EXISTS(TOUCH_INT) + READ(TOUCH_INT_PIN) != HIGH + #else + getRawData(XPT2046_Z1) >= XPT2046_Z1_THRESHOLD + #endif + ); +} + +bool XPT2046::getRawPoint(int16_t *x, int16_t *y) { + if (isBusy()) return false; + if (!isTouched()) return false; + *x = getRawData(XPT2046_X); + *y = getRawData(XPT2046_Y); + return isTouched(); +} + +uint16_t XPT2046::getRawData(const XPTCoordinate coordinate) { + uint16_t data[3]; + + DataTransferBegin(); + + for (uint16_t i = 0; i < 3 ; i++) { + IO(coordinate); + data[i] = (IO() << 4) | (IO() >> 4); + } + + DataTransferEnd(); + + uint16_t delta01 = delta(data[0], data[1]); + uint16_t delta02 = delta(data[0], data[2]); + uint16_t delta12 = delta(data[1], data[2]); + + if (delta01 > delta02 || delta01 > delta12) { + if (delta02 > delta12) + data[0] = data[2]; + else + data[1] = data[2]; + } + + return (data[0] + data[1]) >> 1; +} + +uint16_t XPT2046::HardwareIO(uint16_t data) { + __HAL_SPI_ENABLE(&SPIx); + while ((SPIx.Instance->SR & SPI_FLAG_TXE) != SPI_FLAG_TXE) {} + SPIx.Instance->DR = data; + while ((SPIx.Instance->SR & SPI_FLAG_RXNE) != SPI_FLAG_RXNE) {} + __HAL_SPI_DISABLE(&SPIx); + + return SPIx.Instance->DR; +} + +uint16_t XPT2046::SoftwareIO(uint16_t data) { + uint16_t result = 0; + + for (uint8_t j = 0x80; j > 0; j >>= 1) { + WRITE(TOUCH_SCK_PIN, LOW); + __DSB(); + WRITE(TOUCH_MOSI_PIN, data & j ? HIGH : LOW); + __DSB(); + if (READ(TOUCH_MISO_PIN)) result |= j; + __DSB(); + WRITE(TOUCH_SCK_PIN, HIGH); + __DSB(); + } + WRITE(TOUCH_SCK_PIN, LOW); + __DSB(); + + return result; +} + +#endif // HAS_TFT_XPT2046 +#endif // HAL_STM32 diff --git a/src/HAL/STM32/tft/xpt2046.h b/src/HAL/STM32/tft/xpt2046.h new file mode 100644 index 0000000..71de6b0 --- /dev/null +++ b/src/HAL/STM32/tft/xpt2046.h @@ -0,0 +1,81 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifdef STM32F1xx + #include +#elif defined(STM32F4xx) + #include +#endif + +#include "../../../inc/MarlinConfig.h" + +// Not using regular SPI interface by default to avoid SPI mode conflicts with other SPI devices + +#if !PIN_EXISTS(TOUCH_MISO) + #error "TOUCH_MISO_PIN is not defined." +#elif !PIN_EXISTS(TOUCH_MOSI) + #error "TOUCH_MOSI_PIN is not defined." +#elif !PIN_EXISTS(TOUCH_SCK) + #error "TOUCH_SCK_PIN is not defined." +#elif !PIN_EXISTS(TOUCH_CS) + #error "TOUCH_CS_PIN is not defined." +#endif + +#ifndef TOUCH_INT_PIN + #define TOUCH_INT_PIN -1 +#endif + +#define XPT2046_DFR_MODE 0x00 +#define XPT2046_SER_MODE 0x04 +#define XPT2046_CONTROL 0x80 + +enum XPTCoordinate : uint8_t { + XPT2046_X = 0x10 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Y = 0x50 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z1 = 0x30 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, +}; + +#ifndef XPT2046_Z1_THRESHOLD + #define XPT2046_Z1_THRESHOLD 10 +#endif + +class XPT2046 { +private: + static SPI_HandleTypeDef SPIx; + + static bool isBusy() { return false; } + + static uint16_t getRawData(const XPTCoordinate coordinate); + static bool isTouched(); + + static void DataTransferBegin() { if (SPIx.Instance) { HAL_SPI_Init(&SPIx); } WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + static uint16_t HardwareIO(uint16_t data); + static uint16_t SoftwareIO(uint16_t data); + static uint16_t IO(uint16_t data = 0) { return SPIx.Instance ? HardwareIO(data) : SoftwareIO(data); } + +public: + static void Init(); + static bool getRawPoint(int16_t *x, int16_t *y); +}; diff --git a/src/HAL/STM32/timers.cpp b/src/HAL/STM32/timers.cpp new file mode 100644 index 0000000..e68b59c --- /dev/null +++ b/src/HAL/STM32/timers.cpp @@ -0,0 +1,330 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +// ------------------------ +// Local defines +// ------------------------ + +// Default timer priorities. Override by specifying alternate priorities in the board pins file. +// The TONE timer is not present here, as it currently cannot be set programmatically. It is set +// by defining TIM_IRQ_PRIO in the variant.h or platformio.ini file, which adjusts the default +// priority for STM32 HardwareTimer objects. +#define SWSERIAL_TIMER_IRQ_PRIO_DEFAULT 1 // Requires tight bit timing to communicate reliably with TMC drivers +#define SERVO_TIMER_IRQ_PRIO_DEFAULT 1 // Requires tight PWM timing to control a BLTouch reliably +#define STEP_TIMER_IRQ_PRIO_DEFAULT 2 +#define TEMP_TIMER_IRQ_PRIO_DEFAULT 14 // Low priority avoids interference with other hardware and timers + +#ifndef STEP_TIMER_IRQ_PRIO + #define STEP_TIMER_IRQ_PRIO STEP_TIMER_IRQ_PRIO_DEFAULT +#endif +#ifndef TEMP_TIMER_IRQ_PRIO + #define TEMP_TIMER_IRQ_PRIO TEMP_TIMER_IRQ_PRIO_DEFAULT +#endif +#if HAS_TMC_SW_SERIAL + #include + #ifndef SWSERIAL_TIMER_IRQ_PRIO + #define SWSERIAL_TIMER_IRQ_PRIO SWSERIAL_TIMER_IRQ_PRIO_DEFAULT + #endif +#endif +#if HAS_SERVOS + #include "Servo.h" + #ifndef SERVO_TIMER_IRQ_PRIO + #define SERVO_TIMER_IRQ_PRIO SERVO_TIMER_IRQ_PRIO_DEFAULT + #endif +#endif +#if ENABLED(SPEAKER) + // Ensure the default timer priority is somewhere between the STEP and TEMP priorities. + // The STM32 framework defaults to interrupt 14 for all timers. This should be increased so that + // timing-sensitive operations such as speaker output are not impacted by the long-running + // temperature ISR. This must be defined in the platformio.ini file or the board's variant.h, + // so that it will be consumed by framework code. + #if !(TIM_IRQ_PRIO > STEP_TIMER_IRQ_PRIO && TIM_IRQ_PRIO < TEMP_TIMER_IRQ_PRIO) + #error "Default timer interrupt priority is unspecified or set to a value which may degrade performance." + #endif +#endif + +#if defined(STM32F0xx) || defined(STM32G0xx) + #define MCU_STEP_TIMER 16 + #define MCU_TEMP_TIMER 17 +#elif defined(STM32F1xx) + #define MCU_STEP_TIMER 4 + #define MCU_TEMP_TIMER 2 +#elif defined(STM32F401xC) || defined(STM32F401xE) + #define MCU_STEP_TIMER 9 // STM32F401 has no TIM6, TIM7, or TIM8 + #define MCU_TEMP_TIMER 10 +#elif defined(STM32F4xx) || defined(STM32F7xx) || defined(STM32H7xx) + #define MCU_STEP_TIMER 6 + #define MCU_TEMP_TIMER 14 // TIM7 is consumed by Software Serial if used. +#endif + +#ifndef HAL_TIMER_RATE + #define HAL_TIMER_RATE GetStepperTimerClkFreq() +#endif + +#ifndef STEP_TIMER + #define STEP_TIMER MCU_STEP_TIMER +#endif +#ifndef TEMP_TIMER + #define TEMP_TIMER MCU_TEMP_TIMER +#endif + +#define __TIMER_DEV(X) TIM##X +#define _TIMER_DEV(X) __TIMER_DEV(X) +#define STEP_TIMER_DEV _TIMER_DEV(STEP_TIMER) +#define TEMP_TIMER_DEV _TIMER_DEV(TEMP_TIMER) + +// -------------------------------------------------------------------------- +// Local defines +// -------------------------------------------------------------------------- + +#define NUM_HARDWARE_TIMERS 2 + +// -------------------------------------------------------------------------- +// Private Variables +// -------------------------------------------------------------------------- + +HardwareTimer *timer_instance[NUM_HARDWARE_TIMERS] = { nullptr }; + +// ------------------------ +// Public functions +// ------------------------ + +uint32_t GetStepperTimerClkFreq() { + // Timer input clocks vary between devices, and in some cases between timers on the same device. + // Retrieve at runtime to ensure device compatibility. Cache result to avoid repeated overhead. + static uint32_t clkfreq = timer_instance[MF_TIMER_STEP]->getTimerClkFreq(); + return clkfreq; +} + +// frequency is in Hertz +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + if (!HAL_timer_initialized(timer_num)) { + switch (timer_num) { + case MF_TIMER_STEP: // STEPPER TIMER - use a 32bit timer if possible + timer_instance[timer_num] = new HardwareTimer(STEP_TIMER_DEV); + /* Set the prescaler to the final desired value. + * This will change the effective ISR callback frequency but when + * HAL_timer_start(timer_num=0) is called in the core for the first time + * the real frequency isn't important as long as, after boot, the ISR + * gets called with the correct prescaler and count register. So here + * we set the prescaler to the correct, final value and ignore the frequency + * asked. We will call back the ISR in 1 second to start at full speed. + * + * The proper fix, however, would be a correct initialization OR a + * HAL_timer_change(const uint8_t timer_num, const uint32_t frequency) + * which changes the prescaler when an IRQ frequency change is needed + * (for example when steppers are turned on) + */ + + timer_instance[timer_num]->setPrescaleFactor(STEPPER_TIMER_PRESCALE); //the -1 is done internally + timer_instance[timer_num]->setOverflow(_MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (HAL_TIMER_RATE) / (STEPPER_TIMER_PRESCALE) /* /frequency */), TICK_FORMAT); + break; + case MF_TIMER_TEMP: // TEMP TIMER - any available 16bit timer + timer_instance[timer_num] = new HardwareTimer(TEMP_TIMER_DEV); + // The prescale factor is computed automatically for HERTZ_FORMAT + timer_instance[timer_num]->setOverflow(frequency, HERTZ_FORMAT); + break; + } + + // Disable preload. Leaving it default-enabled can cause the timer to stop if it happens + // to exit the ISR after the start time for the next interrupt has already passed. + timer_instance[timer_num]->setPreloadEnable(false); + + HAL_timer_enable_interrupt(timer_num); + + // Start the timer. + timer_instance[timer_num]->resume(); // First call to resume() MUST follow the attachInterrupt() + + // This is fixed in Arduino_Core_STM32 1.8. + // These calls can be removed and replaced with + // timer_instance[timer_num]->setInterruptPriority + switch (timer_num) { + case MF_TIMER_STEP: + timer_instance[timer_num]->setInterruptPriority(STEP_TIMER_IRQ_PRIO, 0); + break; + case MF_TIMER_TEMP: + timer_instance[timer_num]->setInterruptPriority(TEMP_TIMER_IRQ_PRIO, 0); + break; + } + } +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + if (HAL_timer_initialized(timer_num) && !timer_instance[timer_num]->hasInterrupt()) { + switch (timer_num) { + case MF_TIMER_STEP: + timer_instance[timer_num]->attachInterrupt(Step_Handler); + break; + case MF_TIMER_TEMP: + timer_instance[timer_num]->attachInterrupt(Temp_Handler); + break; + } + } +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + if (HAL_timer_initialized(timer_num)) timer_instance[timer_num]->detachInterrupt(); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + return HAL_timer_initialized(timer_num) && timer_instance[timer_num]->hasInterrupt(); +} + +void SetTimerInterruptPriorities() { + TERN_(HAS_TMC_SW_SERIAL, SoftwareSerial::setInterruptPriority(SWSERIAL_TIMER_IRQ_PRIO, 0)); + TERN_(HAS_SERVOS, libServo::setInterruptPriority(SERVO_TIMER_IRQ_PRIO, 0)); +} + +// ------------------------ +// Detect timer conflicts +// ------------------------ + +// This list serves two purposes. Firstly, it facilitates build-time mapping between +// variant-defined timer names (such as TIM1) and timer numbers. It also replicates +// the order of timers used in the framework's SoftwareSerial.cpp. The first timer in +// this list will be automatically used by SoftwareSerial if it is not already defined +// in the board's variant or compiler options. +static constexpr struct {uintptr_t base_address; int timer_number;} stm32_timer_map[] = { + #ifdef TIM18_BASE + { uintptr_t(TIM18), 18 }, + #endif + #ifdef TIM7_BASE + { uintptr_t(TIM7), 7 }, + #endif + #ifdef TIM6_BASE + { uintptr_t(TIM6), 6 }, + #endif + #ifdef TIM22_BASE + { uintptr_t(TIM22), 22 }, + #endif + #ifdef TIM21_BASE + { uintptr_t(TIM21), 21 }, + #endif + #ifdef TIM17_BASE + { uintptr_t(TIM17), 17 }, + #endif + #ifdef TIM16_BASE + { uintptr_t(TIM16), 16 }, + #endif + #ifdef TIM15_BASE + { uintptr_t(TIM15), 15 }, + #endif + #ifdef TIM14_BASE + { uintptr_t(TIM14), 14 }, + #endif + #ifdef TIM13_BASE + { uintptr_t(TIM13), 13 }, + #endif + #ifdef TIM11_BASE + { uintptr_t(TIM11), 11 }, + #endif + #ifdef TIM10_BASE + { uintptr_t(TIM10), 10 }, + #endif + #ifdef TIM12_BASE + { uintptr_t(TIM12), 12 }, + #endif + #ifdef TIM19_BASE + { uintptr_t(TIM19), 19 }, + #endif + #ifdef TIM9_BASE + { uintptr_t(TIM9), 9 }, + #endif + #ifdef TIM5_BASE + { uintptr_t(TIM5), 5 }, + #endif + #ifdef TIM4_BASE + { uintptr_t(TIM4), 4 }, + #endif + #ifdef TIM3_BASE + { uintptr_t(TIM3), 3 }, + #endif + #ifdef TIM2_BASE + { uintptr_t(TIM2), 2 }, + #endif + #ifdef TIM20_BASE + { uintptr_t(TIM20), 20 }, + #endif + #ifdef TIM8_BASE + { uintptr_t(TIM8), 8 }, + #endif + #ifdef TIM1_BASE + { uintptr_t(TIM1), 1 } + #endif +}; + +// Convert from a timer base address to its integer timer number. +static constexpr int get_timer_num_from_base_address(uintptr_t base_address) { + for (const auto &timer : stm32_timer_map) + if (timer.base_address == base_address) return timer.timer_number; + return 0; +} + +// The platform's SoftwareSerial.cpp will use the first timer from stm32_timer_map. +#if HAS_TMC_SW_SERIAL && !defined(TIMER_SERIAL) + #define TIMER_SERIAL (stm32_timer_map[0].base_address) +#endif + +// constexpr doesn't like using the base address pointers that timers evaluate to. +// We can get away with casting them to uintptr_t, if we do so inside an array. +// GCC will not currently do it directly to a uintptr_t. +IF_ENABLED(HAS_TMC_SW_SERIAL, static constexpr uintptr_t timer_serial[] = {uintptr_t(TIMER_SERIAL)}); +IF_ENABLED(SPEAKER, static constexpr uintptr_t timer_tone[] = {uintptr_t(TIMER_TONE)}); +IF_ENABLED(HAS_SERVOS, static constexpr uintptr_t timer_servo[] = {uintptr_t(TIMER_SERVO)}); + +enum TimerPurpose { TP_SERIAL, TP_TONE, TP_SERVO, TP_STEP, TP_TEMP }; + +// List of timers, to enable checking for conflicts. +// Includes the purpose of each timer to ease debugging when evaluating at build-time. +// This cannot yet account for timers used for PWM output, such as for fans. +static constexpr struct { TimerPurpose p; int t; } timers_in_use[] = { + #if HAS_TMC_SW_SERIAL + { TP_SERIAL, get_timer_num_from_base_address(timer_serial[0]) }, // Set in variant.h, or as a define in platformio.h if not present in variant.h + #endif + #if ENABLED(SPEAKER) + { TP_TONE, get_timer_num_from_base_address(timer_tone[0]) }, // Set in variant.h, or as a define in platformio.h if not present in variant.h + #endif + #if HAS_SERVOS + { TP_SERVO, get_timer_num_from_base_address(timer_servo[0]) }, // Set in variant.h, or as a define in platformio.h if not present in variant.h + #endif + { TP_STEP, STEP_TIMER }, + { TP_TEMP, TEMP_TIMER }, +}; + +static constexpr bool verify_no_timer_conflicts() { + LOOP_L_N(i, COUNT(timers_in_use)) + LOOP_S_L_N(j, i + 1, COUNT(timers_in_use)) + if (timers_in_use[i].t == timers_in_use[j].t) return false; + return true; +} + +// If this assertion fails at compile time, review the timers_in_use array. +// If default_envs is defined properly in platformio.ini, VS Code can evaluate the array +// when hovering over it, making it easy to identify the conflicting timers. +static_assert(verify_no_timer_conflicts(), "One or more timer conflict detected. Examine \"timers_in_use\" to help identify conflict."); + +#endif // HAL_STM32 diff --git a/src/HAL/STM32/timers.h b/src/HAL/STM32/timers.h new file mode 100644 index 0000000..6828998 --- /dev/null +++ b/src/HAL/STM32/timers.h @@ -0,0 +1,120 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" + +// ------------------------ +// Defines +// ------------------------ + +// STM32 timers may be 16 or 32 bit. Limiting HAL_TIMER_TYPE_MAX to 16 bits +// avoids issues with STM32F0 MCUs, which seem to pause timers if UINT32_MAX +// is written to the register. STM32F4 timers do not manifest this issue, +// even when writing to 16 bit timers. +// +// The range of the timer can be queried at runtime using IS_TIM_32B_COUNTER_INSTANCE. +// This is a more expensive check than a simple compile-time constant, so its +// implementation is deferred until the desire for a 32-bit range outweighs the cost +// of adding a run-time check and HAL_TIMER_TYPE_MAX is refactored to allow unique +// values for each timer. +#define hal_timer_t uint32_t +#define HAL_TIMER_TYPE_MAX UINT16_MAX + +// Marlin timer_instance[] content (unrelated to timer selection) +#define MF_TIMER_STEP 0 // Timer Index for Stepper +#define MF_TIMER_TEMP 1 // Timer Index for Temperature +#define MF_TIMER_PULSE MF_TIMER_STEP + +#define TIMER_INDEX_(T) TIMER##T##_INDEX // TIMER#_INDEX enums (timer_index_t) depend on TIM#_BASE defines. +#define TIMER_INDEX(T) TIMER_INDEX_(T) // Convert Timer ID to HardwareTimer_Handle index. + +#define TEMP_TIMER_FREQUENCY 1000 // Temperature::isr() is expected to be called at around 1kHz + +// TODO: get rid of manual rate/prescale/ticks/cycles taken for procedures in stepper.cpp +#define STEPPER_TIMER_RATE 2000000 // 2 Mhz +extern uint32_t GetStepperTimerClkFreq(); +#define STEPPER_TIMER_PRESCALE (GetStepperTimerClkFreq() / (STEPPER_TIMER_RATE)) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +extern void Step_Handler(); +extern void Temp_Handler(); + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() void Step_Handler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() void Temp_Handler() +#endif + +// ------------------------ +// Public Variables +// ------------------------ + +extern HardwareTimer *timer_instance[]; + +// ------------------------ +// Public functions +// ------------------------ + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +// Configure timer priorities for peripherals such as Software Serial or Servos. +// Exposed here to allow all timer priority information to reside in timers.cpp +void SetTimerInterruptPriorities(); + +// FORCE_INLINE because these are used in performance-critical situations +FORCE_INLINE bool HAL_timer_initialized(const uint8_t timer_num) { + return timer_instance[timer_num] != nullptr; +} +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + return HAL_timer_initialized(timer_num) ? timer_instance[timer_num]->getCount() : 0; +} + +// NOTE: Method name may be misleading. +// STM32 has an Auto-Reload Register (ARR) as opposed to a "compare" register +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t overflow) { + if (HAL_timer_initialized(timer_num)) { + timer_instance[timer_num]->setOverflow(overflow + 1, TICK_FORMAT); // Value decremented by setOverflow() + // wiki: "force all registers (Autoreload, prescaler, compare) to be taken into account" + // So, if the new overflow value is less than the count it will trigger a rollover interrupt. + if (overflow < timer_instance[timer_num]->getCount()) // Added 'if' here because reports say it won't boot without it + timer_instance[timer_num]->refresh(); + } +} + +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/STM32/usb_host.cpp b/src/HAL/STM32/usb_host.cpp new file mode 100644 index 0000000..d77f0b2 --- /dev/null +++ b/src/HAL/STM32/usb_host.cpp @@ -0,0 +1,119 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if BOTH(USE_OTG_USB_HOST, USBHOST) + +#include "usb_host.h" +#include "../shared/Marduino.h" +#include "usbh_core.h" +#include "usbh_msc.h" + +USBH_HandleTypeDef hUsbHost; +USBHost usb; +BulkStorage bulk(&usb); + +static void USBH_UserProcess(USBH_HandleTypeDef *phost, uint8_t id) { + switch(id) { + case HOST_USER_SELECT_CONFIGURATION: + //SERIAL_ECHOLNPGM("APPLICATION_SELECT_CONFIGURATION"); + break; + case HOST_USER_DISCONNECTION: + //SERIAL_ECHOLNPGM("APPLICATION_DISCONNECT"); + //usb.setUsbTaskState(USB_STATE_RUNNING); + break; + case HOST_USER_CLASS_ACTIVE: + //SERIAL_ECHOLNPGM("APPLICATION_READY"); + usb.setUsbTaskState(USB_STATE_RUNNING); + break; + case HOST_USER_CONNECTION: + break; + default: + break; + } +} + +bool USBHost::start() { + if (USBH_Init(&hUsbHost, USBH_UserProcess, TERN(USE_USB_HS_IN_FS, HOST_HS, HOST_FS)) != USBH_OK) { + SERIAL_ECHOLNPGM("Error: USBH_Init"); + return false; + } + if (USBH_RegisterClass(&hUsbHost, USBH_MSC_CLASS) != USBH_OK) { + SERIAL_ECHOLNPGM("Error: USBH_RegisterClass"); + return false; + } + if (USBH_Start(&hUsbHost) != USBH_OK) { + SERIAL_ECHOLNPGM("Error: USBH_Start"); + return false; + } + return true; +} + +void USBHost::Task() { + USBH_Process(&hUsbHost); +} + +uint8_t USBHost::getUsbTaskState() { + return usb_task_state; +} + +void USBHost::setUsbTaskState(uint8_t state) { + usb_task_state = state; + if (usb_task_state == USB_STATE_RUNNING) { + MSC_LUNTypeDef info; + USBH_MSC_GetLUNInfo(&hUsbHost, usb.lun, &info); + capacity = info.capacity.block_nbr / 2000; + block_size = info.capacity.block_size; + block_count = info.capacity.block_nbr; + //SERIAL_ECHOLNPGM("info.capacity.block_nbr : %ld\n", info.capacity.block_nbr); + //SERIAL_ECHOLNPGM("info.capacity.block_size: %d\n", info.capacity.block_size); + //SERIAL_ECHOLNPGM("capacity : %d MB\n", capacity); + } +}; + +bool BulkStorage::LUNIsGood(uint8_t t) { + return USBH_MSC_IsReady(&hUsbHost) && USBH_MSC_UnitIsReady(&hUsbHost, t); +} + +uint32_t BulkStorage::GetCapacity(uint8_t lun) { + return usb->block_count; +} + +uint16_t BulkStorage::GetSectorSize(uint8_t lun) { + return usb->block_size; +} + +uint8_t BulkStorage::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, uint8_t *buf) { + return USBH_MSC_Read(&hUsbHost, lun, addr, buf, blocks) != USBH_OK; +} + +uint8_t BulkStorage::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, const uint8_t * buf) { + return USBH_MSC_Write(&hUsbHost, lun, addr, const_cast(buf), blocks) != USBH_OK; +} + +#endif // USE_OTG_USB_HOST && USBHOST +#endif // HAL_STM32 diff --git a/src/HAL/STM32/usb_host.h b/src/HAL/STM32/usb_host.h new file mode 100644 index 0000000..c0001c0 --- /dev/null +++ b/src/HAL/STM32/usb_host.h @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +typedef enum { + USB_STATE_INIT, + USB_STATE_ERROR, + USB_STATE_RUNNING, +} usb_state_t; + +class USBHost { +public: + bool start(); + void Task(); + uint8_t getUsbTaskState(); + void setUsbTaskState(uint8_t state); + uint8_t regRd(uint8_t reg) { return 0x0; }; + uint8_t usb_task_state = USB_STATE_INIT; + uint8_t lun = 0; + uint32_t capacity = 0; + uint16_t block_size = 0; + uint32_t block_count = 0; +}; + +class BulkStorage { +public: + BulkStorage(USBHost *usb) : usb(usb) {}; + + bool LUNIsGood(uint8_t t); + uint32_t GetCapacity(uint8_t lun); + uint16_t GetSectorSize(uint8_t lun); + uint8_t Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, uint8_t *buf); + uint8_t Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, const uint8_t * buf); + + USBHost *usb; +}; + +extern USBHost usb; +extern BulkStorage bulk; diff --git a/src/HAL/STM32/usb_serial.cpp b/src/HAL/STM32/usb_serial.cpp new file mode 100644 index 0000000..0b2372f --- /dev/null +++ b/src/HAL/STM32/usb_serial.cpp @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(EMERGENCY_PARSER) && (USBD_USE_CDC || USBD_USE_CDC_MSC) + +#include "usb_serial.h" +#include "../../feature/e_parser.h" + +EmergencyParser::State emergency_state = EmergencyParser::State::EP_RESET; + +int8_t (*USBD_CDC_Receive_original) (uint8_t *Buf, uint32_t *Len) = nullptr; + +static int8_t USBD_CDC_Receive_hook(uint8_t *Buf, uint32_t *Len) { + for (uint32_t i = 0; i < *Len; i++) + emergency_parser.update(emergency_state, Buf[i]); + return USBD_CDC_Receive_original(Buf, Len); +} + +typedef struct _USBD_CDC_Itf { + int8_t (* Init)(void); + int8_t (* DeInit)(void); + int8_t (* Control)(uint8_t cmd, uint8_t *pbuf, uint16_t length); + int8_t (* Receive)(uint8_t *Buf, uint32_t *Len); + int8_t (* Transferred)(void); +} USBD_CDC_ItfTypeDef; + +extern USBD_CDC_ItfTypeDef USBD_CDC_fops; + +void USB_Hook_init() { + USBD_CDC_Receive_original = USBD_CDC_fops.Receive; + USBD_CDC_fops.Receive = USBD_CDC_Receive_hook; +} + +#endif // EMERGENCY_PARSER && USBD_USE_CDC +#endif // HAL_STM32 diff --git a/src/HAL/STM32/usb_serial.h b/src/HAL/STM32/usb_serial.h new file mode 100644 index 0000000..3edb6fd --- /dev/null +++ b/src/HAL/STM32/usb_serial.h @@ -0,0 +1,24 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void USB_Hook_init(); diff --git a/src/HAL/STM32F1/HAL.cpp b/src/HAL/STM32F1/HAL.cpp new file mode 100644 index 0000000..4d31400 --- /dev/null +++ b/src/HAL/STM32F1/HAL.cpp @@ -0,0 +1,387 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include + +// ------------------------ +// Types +// ------------------------ + +#define __I +#define __IO volatile + typedef struct { + __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ + __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ + __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ + __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ + __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ + __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ + __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ + __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ + __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ + __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ + __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ + __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ + __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ + __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ + __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ + __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ + __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ + __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ + __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ + uint32_t RESERVED0[5]; + __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ + } SCB_Type; + +// ------------------------ +// Local defines +// ------------------------ + +#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ +#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ + +#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ + +/* SCB Application Interrupt and Reset Control Register Definitions */ +#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */ +#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ + +#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */ +#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ + +// ------------------------ +// Serial ports +// ------------------------ + +#if defined(SERIAL_USB) && !HAS_SD_HOST_DRIVE + + USBSerial SerialUSB; + DefaultSerial1 MSerial0(true, SerialUSB); + + #if ENABLED(EMERGENCY_PARSER) + #include "../libmaple/usb/stm32f1/usb_reg_map.h" + #include "libmaple/usb_cdcacm.h" + // The original callback is not called (no way to retrieve address). + // That callback detects a special STM32 reset sequence: this functionality is not essential + // as M997 achieves the same. + void my_rx_callback(unsigned int, void*) { + // max length of 16 is enough to contain all emergency commands + uint8 buf[16]; + + //rx is usbSerialPart.endpoints[2] + uint16 len = usb_get_ep_rx_count(USB_CDCACM_RX_ENDP); + uint32 total = usb_cdcacm_data_available(); + + if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf))) + return; + + // cannot get character by character due to bug in composite_cdcacm_peek_ex + len = usb_cdcacm_peek(buf, total); + + for (uint32 i = 0; i < len; i++) + emergency_parser.update(MSerial0.emergency_state, buf[i + total - len]); + } + #endif +#endif + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #include + + void watchdogSetup() { + // do whatever. don't remove this function. + } + + /** + * The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0). + */ + #define STM32F1_WD_RELOAD TERN(WATCHDOG_DURATION_8S, 1250, 625) // 4 or 8 second timeout + + /** + * @brief Initialize the independent hardware watchdog. + * + * @return No return + * + * @details The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0). + */ + void MarlinHAL::watchdog_init() { + #if DISABLED(DISABLE_WATCHDOG_INIT) + iwdg_init(IWDG_PRE_256, STM32F1_WD_RELOAD); + #endif + } + + // Reset watchdog. MUST be called every 4 or 8 seconds after the + // first watchdog_init or the STM32F1 will reset. + void MarlinHAL::watchdog_refresh() { + #if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED) + TOGGLE(LED_PIN); // heartbeat indicator + #endif + iwdg_feed(); + } + +#endif // USE_WATCHDOG + +// ------------------------ +// ADC +// ------------------------ + +// Watch out for recursion here! Our pin_t is signed, so pass through to Arduino -> analogRead(uint8_t) + +uint16_t analogRead(const pin_t pin) { + const bool is_analog = _GET_MODE(pin) == GPIO_INPUT_ANALOG; + return is_analog ? analogRead(uint8_t(pin)) : 0; +} + +// Wrapper to maple unprotected analogWrite +void analogWrite(const pin_t pin, int pwm_val8) { + if (PWM_PIN(pin)) analogWrite(uint8_t(pin), pwm_val8); +} + +uint16_t MarlinHAL::adc_result; + +// ------------------------ +// Private functions +// ------------------------ + +static void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) { + uint32_t reg_value; + uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); // only values 0..7 are used + + reg_value = SCB->AIRCR; // read old register configuration + reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); // clear bits to change + reg_value = (reg_value | + ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | + (PriorityGroupTmp << 8)); // Insert write key & priority group + SCB->AIRCR = reg_value; +} + +// ------------------------ +// Public functions +// ------------------------ + +void flashFirmware(const int16_t) { hal.reboot(); } + +// +// Leave PA11/PA12 intact if USBSerial is not used +// +#if SERIAL_USB + namespace wirish { namespace priv { + #if SERIAL_PORT > 0 + #if SERIAL_PORT2 + #if SERIAL_PORT2 > 0 + void board_setup_usb() {} + #endif + #else + void board_setup_usb() {} + #endif + #endif + } } +#endif + +TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); + +// ------------------------ +// MarlinHAL class +// ------------------------ + +void MarlinHAL::init() { + NVIC_SetPriorityGrouping(0x3); + #if PIN_EXISTS(LED) + OUT_WRITE(LED_PIN, LOW); + #endif + #if HAS_SD_HOST_DRIVE + MSC_SD_init(); + #elif BOTH(SERIAL_USB, EMERGENCY_PARSER) + usb_cdcacm_set_hooks(USB_CDCACM_HOOK_RX, my_rx_callback); + #endif + #if PIN_EXISTS(USB_CONNECT) + OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING); // USB clear connection + delay(1000); // Give OS time to notice + WRITE(USB_CONNECT_PIN, USB_CONNECT_INVERTING); + #endif + TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the minimal serial handler +} + +// HAL idle task +void MarlinHAL::idletask() { + #if HAS_SHARED_MEDIA + // If Marlin is using the SD card we need to lock it to prevent access from + // a PC via USB. + // Other HALs use IS_SD_PRINTING() and IS_SD_FILE_OPEN() to check for access but + // this will not reliably detect delete operations. To be safe we will lock + // the disk if Marlin has it mounted. Unfortunately there is currently no way + // to unmount the disk from the LCD menu. + // if (IS_SD_PRINTING() || IS_SD_FILE_OPEN()) + /* copy from lpc1768 framework, should be fixed later for process HAS_SD_HOST_DRIVE*/ + // process USB mass storage device class loop + MarlinMSC.loop(); + #endif +} + +void MarlinHAL::reboot() { nvic_sys_reset(); } + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +extern "C" { + extern unsigned int _ebss; // end of bss section +} + +/** + * TODO: Change this to correct it for libmaple + */ + +// return free memory between end of heap (or end bss) and whatever is current + +/* +#include +//extern caddr_t _sbrk(int incr); +#ifndef CONFIG_HEAP_END +extern char _lm_heap_end; +#define CONFIG_HEAP_END ((caddr_t)&_lm_heap_end) +#endif + +extern "C" { + static int freeMemory() { + char top = 't'; + return &top - reinterpret_cast(sbrk(0)); + } + int freeMemory() { + int free_memory; + int heap_end = (int)_sbrk(0); + free_memory = ((int)&free_memory) - ((int)heap_end); + return free_memory; + } +} +*/ + +// ------------------------ +// ADC +// ------------------------ + +enum ADCIndex : uint8_t { + OPTITEM(HAS_TEMP_ADC_0, TEMP_0) + OPTITEM(HAS_TEMP_ADC_1, TEMP_1) + OPTITEM(HAS_TEMP_ADC_2, TEMP_2) + OPTITEM(HAS_TEMP_ADC_3, TEMP_3) + OPTITEM(HAS_TEMP_ADC_4, TEMP_4) + OPTITEM(HAS_TEMP_ADC_5, TEMP_5) + OPTITEM(HAS_TEMP_ADC_6, TEMP_6) + OPTITEM(HAS_TEMP_ADC_7, TEMP_7) + OPTITEM(HAS_HEATED_BED, TEMP_BED) + OPTITEM(HAS_TEMP_CHAMBER, TEMP_CHAMBER) + OPTITEM(HAS_TEMP_ADC_PROBE, TEMP_PROBE) + OPTITEM(HAS_TEMP_COOLER, TEMP_COOLER) + OPTITEM(HAS_TEMP_BOARD, TEMP_BOARD) + OPTITEM(FILAMENT_WIDTH_SENSOR, FILWIDTH) + OPTITEM(HAS_ADC_BUTTONS, ADC_KEY) + OPTITEM(HAS_JOY_ADC_X, JOY_X) + OPTITEM(HAS_JOY_ADC_Y, JOY_Y) + OPTITEM(HAS_JOY_ADC_Z, JOY_Z) + OPTITEM(POWER_MONITOR_CURRENT, POWERMON_CURRENT) + OPTITEM(POWER_MONITOR_VOLTAGE, POWERMON_VOLTS) + ADC_COUNT +}; + +static uint16_t adc_results[ADC_COUNT]; + +// Init the AD in continuous capture mode +void MarlinHAL::adc_init() { + static const uint8_t adc_pins[] = { + OPTITEM(HAS_TEMP_ADC_0, TEMP_0_PIN) + OPTITEM(HAS_TEMP_ADC_1, TEMP_1_PIN) + OPTITEM(HAS_TEMP_ADC_2, TEMP_2_PIN) + OPTITEM(HAS_TEMP_ADC_3, TEMP_3_PIN) + OPTITEM(HAS_TEMP_ADC_4, TEMP_4_PIN) + OPTITEM(HAS_TEMP_ADC_5, TEMP_5_PIN) + OPTITEM(HAS_TEMP_ADC_6, TEMP_6_PIN) + OPTITEM(HAS_TEMP_ADC_7, TEMP_7_PIN) + OPTITEM(HAS_HEATED_BED, TEMP_BED_PIN) + OPTITEM(HAS_TEMP_CHAMBER, TEMP_CHAMBER_PIN) + OPTITEM(HAS_TEMP_ADC_PROBE, TEMP_PROBE_PIN) + OPTITEM(HAS_TEMP_COOLER, TEMP_COOLER_PIN) + OPTITEM(HAS_TEMP_BOARD, TEMP_BOARD_PIN) + OPTITEM(FILAMENT_WIDTH_SENSOR, FILWIDTH_PIN) + OPTITEM(HAS_ADC_BUTTONS, ADC_KEYPAD_PIN) + OPTITEM(HAS_JOY_ADC_X, JOY_X_PIN) + OPTITEM(HAS_JOY_ADC_Y, JOY_Y_PIN) + OPTITEM(HAS_JOY_ADC_Z, JOY_Z_PIN) + OPTITEM(POWER_MONITOR_CURRENT, POWER_MONITOR_CURRENT_PIN) + OPTITEM(POWER_MONITOR_VOLTAGE, POWER_MONITOR_VOLTAGE_PIN) + }; + static STM32ADC adc(ADC1); + // configure the ADC + adc.calibrate(); + adc.setSampleRate((F_CPU > 72000000) ? ADC_SMPR_71_5 : ADC_SMPR_41_5); // 71.5 or 41.5 ADC cycles + adc.setPins((uint8_t *)adc_pins, ADC_COUNT); + adc.setDMA(adc_results, uint16_t(ADC_COUNT), uint32_t(DMA_MINC_MODE | DMA_CIRC_MODE), nullptr); + adc.setScanMode(); + adc.setContinuous(); + adc.startConversion(); +} + +void MarlinHAL::adc_start(const pin_t pin) { + #define __TCASE(N,I) case N: pin_index = I; break; + #define _TCASE(C,N,I) TERN_(C, __TCASE(N, I)) + ADCIndex pin_index; + switch (pin) { + default: return; + _TCASE(HAS_TEMP_ADC_0, TEMP_0_PIN, TEMP_0) + _TCASE(HAS_TEMP_ADC_1, TEMP_1_PIN, TEMP_1) + _TCASE(HAS_TEMP_ADC_2, TEMP_2_PIN, TEMP_2) + _TCASE(HAS_TEMP_ADC_3, TEMP_3_PIN, TEMP_3) + _TCASE(HAS_TEMP_ADC_4, TEMP_4_PIN, TEMP_4) + _TCASE(HAS_TEMP_ADC_5, TEMP_5_PIN, TEMP_5) + _TCASE(HAS_TEMP_ADC_6, TEMP_6_PIN, TEMP_6) + _TCASE(HAS_TEMP_ADC_7, TEMP_7_PIN, TEMP_7) + _TCASE(HAS_HEATED_BED, TEMP_BED_PIN, TEMP_BED) + _TCASE(HAS_TEMP_CHAMBER, TEMP_CHAMBER_PIN, TEMP_CHAMBER) + _TCASE(HAS_TEMP_ADC_PROBE, TEMP_PROBE_PIN, TEMP_PROBE) + _TCASE(HAS_TEMP_COOLER, TEMP_COOLER_PIN, TEMP_COOLER) + _TCASE(HAS_TEMP_BOARD, TEMP_BOARD_PIN, TEMP_BOARD) + _TCASE(HAS_JOY_ADC_X, JOY_X_PIN, JOY_X) + _TCASE(HAS_JOY_ADC_Y, JOY_Y_PIN, JOY_Y) + _TCASE(HAS_JOY_ADC_Z, JOY_Z_PIN, JOY_Z) + _TCASE(FILAMENT_WIDTH_SENSOR, FILWIDTH_PIN, FILWIDTH) + _TCASE(HAS_ADC_BUTTONS, ADC_KEYPAD_PIN, ADC_KEY) + _TCASE(POWER_MONITOR_CURRENT, POWER_MONITOR_CURRENT_PIN, POWERMON_CURRENT) + _TCASE(POWER_MONITOR_VOLTAGE, POWER_MONITOR_VOLTAGE_PIN, POWERMON_VOLTS) + } + adc_result = (adc_results[(int)pin_index] & 0xFFF) >> (12 - HAL_ADC_RESOLUTION); // shift out unused bits +} + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/HAL.h b/src/HAL/STM32F1/HAL.h new file mode 100644 index 0000000..b14b5f7 --- /dev/null +++ b/src/HAL/STM32F1/HAL.h @@ -0,0 +1,309 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + */ + +#define CPU_32_BIT + +#include "../../core/macros.h" +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" + +#include "fastio.h" + +#include +#include + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_SD_HOST_DRIVE + #include "msc_sd.h" +#endif + +#include "MarlinSerial.h" + +// ------------------------ +// Defines +// ------------------------ + +// +// Default graphical display delays +// +#define CPU_ST7920_DELAY_1 300 +#define CPU_ST7920_DELAY_2 40 +#define CPU_ST7920_DELAY_3 340 + +#ifndef STM32_FLASH_SIZE + #if ANY(MCU_STM32F103RE, MCU_STM32F103VE, MCU_STM32F103ZE) + #define STM32_FLASH_SIZE 512 + #else + #define STM32_FLASH_SIZE 256 + #endif +#endif + +// ------------------------ +// Serial ports +// ------------------------ + +#ifdef SERIAL_USB + typedef ForwardSerial1Class< USBSerial > DefaultSerial1; + extern DefaultSerial1 MSerial0; + #if HAS_SD_HOST_DRIVE + #define UsbSerial MarlinCompositeSerial + #else + #define UsbSerial MSerial0 + #endif +#endif + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY) + #define NUM_UARTS 5 +#else + #define NUM_UARTS 3 +#endif + +#if SERIAL_PORT == -1 + #define MYSERIAL1 UsbSerial +#elif WITHIN(SERIAL_PORT, 1, NUM_UARTS) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #define MYSERIAL1 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") +#endif + +#ifdef SERIAL_PORT_2 + #if SERIAL_PORT_2 == -1 + #define MYSERIAL2 UsbSerial + #elif WITHIN(SERIAL_PORT_2, 1, NUM_UARTS) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #define MYSERIAL2 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT_2 must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 + #define MYSERIAL3 UsbSerial + #elif WITHIN(SERIAL_PORT_3, 1, NUM_UARTS) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #else + #define MYSERIAL3 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT_3 must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if MMU2_SERIAL_PORT == -1 + #define MMU2_SERIAL UsbSerial + #elif WITHIN(MMU2_SERIAL_PORT, 1, NUM_UARTS) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #define MMU2_SERIAL MSERIAL(1) // dummy port + static_assert(false, "MMU2_SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if LCD_SERIAL_PORT == -1 + #define LCD_SERIAL UsbSerial + #elif WITHIN(LCD_SERIAL_PORT, 1, NUM_UARTS) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #define LCD_SERIAL MSERIAL(1) // dummy port + static_assert(false, "LCD_SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") + #endif + #if HAS_DGUS_LCD + #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite() + #endif +#endif + +/** + * TODO: review this to return 1 for pins that are not analog input + */ +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) (p) +#endif + +#ifndef digitalPinHasPWM + #define digitalPinHasPWM(P) !!PIN_MAP[P].timer_device + #define NO_COMPILE_TIME_PWM +#endif + +// Reset Reason +#define RST_POWER_ON 1 +#define RST_EXTERNAL 2 +#define RST_BROWN_OUT 4 +#define RST_WATCHDOG 8 +#define RST_JTAG 16 +#define RST_SOFTWARE 32 +#define RST_BACKUP 64 + +// ------------------------ +// Types +// ------------------------ + +typedef int8_t pin_t; + +// ------------------------ +// Interrupts +// ------------------------ + +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); (void)__iCliRetVal() +#define CRITICAL_SECTION_END() if (!irqon) (void)__iSeiRetVal() +#define cli() noInterrupts() +#define sei() interrupts() + +// ------------------------ +// ADC +// ------------------------ + +#ifdef ADC_RESOLUTION + #define HAL_ADC_RESOLUTION ADC_RESOLUTION +#else + #define HAL_ADC_RESOLUTION 12 +#endif + +#define HAL_ADC_VREF 3.3 + +uint16_t analogRead(const pin_t pin); // need hal.adc_enable() first +void analogWrite(const pin_t pin, int pwm_val8); // PWM only! mul by 257 in maple!? + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +#define JTAG_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY) +#define JTAGSWD_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_NONE) + +#define PLATFORM_M997_SUPPORT +void flashFirmware(const int16_t); + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment +#ifndef PWM_FREQUENCY + #define PWM_FREQUENCY 1000 // Default PWM Frequency +#endif + +// ------------------------ +// Class Utilities +// ------------------------ + +// Memory related +#define __bss_end __bss_end__ + +void _delay_ms(const int ms); + +extern "C" char* _sbrk(int incr); + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +static inline int freeMemory() { + volatile char top; + return &top - _sbrk(0); +} + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_primask(); } + static void isr_on() { ((void)__iSeiRetVal()); } + static void isr_off() { ((void)__iCliRetVal()); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source() { return RST_POWER_ON; } + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { pinMode(pin, INPUT_ANALOG); } + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the maximum size of the provided value to enable finer PWM duty control [default = 255] + * The timer must be pre-configured with set_pwm_frequency() if the default frequency is not desired. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false); + + /** + * Set the frequency of the timer for the given pin. + * All Timer PWM pins run at the same frequency. + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); + +}; diff --git a/src/HAL/STM32F1/HAL_SPI.cpp b/src/HAL/STM32F1/HAL_SPI.cpp new file mode 100644 index 0000000..abb348d --- /dev/null +++ b/src/HAL/STM32F1/HAL_SPI.cpp @@ -0,0 +1,171 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Software SPI functions originally from Arduino Sd2Card Library + * Copyright (c) 2009 by William Greiman + * Adapted to the STM32F1 HAL + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" +#include + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(SOFTWARE_SPI) + + // ------------------------ + // Software SPI + // ------------------------ + #error "Software SPI not supported for STM32F1. Use hardware SPI." + +#else + +// ------------------------ +// Hardware SPI +// ------------------------ + +/** + * VGPV SPI speed start and F_CPU/2, by default 72/2 = 36Mhz + */ + +/** + * @brief Begin SPI port setup + * + * @return Nothing + * + * @details Only configures SS pin since libmaple creates and initialize the SPI object + */ +void spiBegin() { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif +} + +/** + * @brief Initialize SPI port to required speed rate and transfer mode (MSB, SPI MODE 0) + * + * @param spiRate Rate as declared in HAL.h (speed do not match AVR) + * @return Nothing + * + * @details + */ +void spiInit(uint8_t spiRate) { + /** + * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz + * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1 + * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2 + */ + #if SPI_DEVICE == 1 + #define SPI_CLOCK_MAX SPI_CLOCK_DIV4 + #else + #define SPI_CLOCK_MAX SPI_CLOCK_DIV2 + #endif + uint8_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = SPI_CLOCK_MAX ; break; + case SPI_HALF_SPEED: clock = SPI_CLOCK_DIV4 ; break; + case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8 ; break; + case SPI_EIGHTH_SPEED: clock = SPI_CLOCK_DIV16; break; + case SPI_SPEED_5: clock = SPI_CLOCK_DIV32; break; + case SPI_SPEED_6: clock = SPI_CLOCK_DIV64; break; + default: clock = SPI_CLOCK_DIV2; // Default from the SPI library + } + SPI.setModule(SPI_DEVICE); + SPI.begin(); + SPI.setClockDivider(clock); + SPI.setBitOrder(MSBFIRST); + SPI.setDataMode(SPI_MODE0); +} + +/** + * @brief Receive a single byte from the SPI port. + * + * @return Byte received + * + * @details + */ +uint8_t spiRec() { + uint8_t returnByte = SPI.transfer(0xFF); + return returnByte; +} + +/** + * @brief Receive a number of bytes from the SPI port to a buffer + * + * @param buf Pointer to starting address of buffer to write to. + * @param nbyte Number of bytes to receive. + * @return Nothing + * + * @details Uses DMA + */ +void spiRead(uint8_t *buf, uint16_t nbyte) { + SPI.dmaTransfer(0, const_cast(buf), nbyte); +} + +/** + * @brief Send a single byte on SPI port + * + * @param b Byte to send + * + * @details + */ +void spiSend(uint8_t b) { + SPI.send(b); +} + +/** + * @brief Write token and then write from 512 byte buffer to SPI (for SD card) + * + * @param buf Pointer with buffer start address + * @return Nothing + * + * @details Use DMA + */ +void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI.send(token); + SPI.dmaSend(const_cast(buf), 512); +} + +#if ENABLED(SPI_EEPROM) + +// Read single byte from specified SPI channel +uint8_t spiRec(uint32_t chan) { return SPI.transfer(0xFF); } + +// Write single byte to specified SPI channel +void spiSend(uint32_t chan, byte b) { SPI.send(b); } + +// Write buffer to specified SPI channel +void spiSend(uint32_t chan, const uint8_t *buf, size_t n) { + for (size_t p = 0; p < n; p++) spiSend(chan, buf[p]); +} + +#endif // SPI_EEPROM + +#endif // SOFTWARE_SPI + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/MarlinSPI.h b/src/HAL/STM32F1/MarlinSPI.h new file mode 100644 index 0000000..fab245f --- /dev/null +++ b/src/HAL/STM32F1/MarlinSPI.h @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +/** + * Marlin currently requires 3 SPI classes: + * + * SPIClass: + * This class is normally provided by frameworks and has a semi-default interface. + * This is needed because some libraries reference it globally. + * + * SPISettings: + * Container for SPI configs for SPIClass. As above, libraries may reference it globally. + * + * These two classes are often provided by frameworks so we cannot extend them to add + * useful methods for Marlin. + * + * MarlinSPI: + * Provides the default SPIClass interface plus some Marlin goodies such as a simplified + * interface for SPI DMA transfer. + * + */ + +using MarlinSPI = SPIClass; diff --git a/src/HAL/STM32F1/MarlinSerial.cpp b/src/HAL/STM32F1/MarlinSerial.cpp new file mode 100644 index 0000000..6dabcde --- /dev/null +++ b/src/HAL/STM32F1/MarlinSerial.cpp @@ -0,0 +1,204 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" +#include "MarlinSerial.h" +#include + +// Copied from ~/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usart_private.h +// Changed to handle Emergency Parser +static inline __always_inline void my_usart_irq(ring_buffer *rb, ring_buffer *wb, usart_reg_map *regs, MSerialT &serial) { + /* Handle RXNEIE and TXEIE interrupts. + * RXNE signifies availability of a byte in DR. + * + * See table 198 (sec 27.4, p809) in STM document RM0008 rev 15. + * We enable RXNEIE. + */ + uint32_t srflags = regs->SR, cr1its = regs->CR1; + + if ((cr1its & USART_CR1_RXNEIE) && (srflags & USART_SR_RXNE)) { + if (srflags & USART_SR_FE || srflags & USART_SR_PE ) { + // framing error or parity error + regs->DR; // Read and throw away the data, which also clears FE and PE + } + else { + uint8_t c = (uint8)regs->DR; + #ifdef USART_SAFE_INSERT + // If the buffer is full and the user defines USART_SAFE_INSERT, + // ignore new bytes. + rb_safe_insert(rb, c); + #else + // By default, push bytes around in the ring buffer. + rb_push_insert(rb, c); + #endif + #if ENABLED(EMERGENCY_PARSER) + if (serial.emergency_parser_enabled()) + emergency_parser.update(serial.emergency_state, c); + #endif + } + } + else if (srflags & USART_SR_ORE) { + // overrun and empty data, just do a dummy read to clear ORE + // and prevent a raise condition where a continuous interrupt stream (due to ORE set) occurs + // (see chapter "Overrun error" ) in STM32 reference manual + regs->DR; + } + + // TXE signifies readiness to send a byte to DR. + if ((cr1its & USART_CR1_TXEIE) && (srflags & USART_SR_TXE)) { + if (!rb_is_empty(wb)) + regs->DR=rb_remove(wb); + else + regs->CR1 &= ~((uint32)USART_CR1_TXEIE); // disable TXEIE + } +} + +// Not every MarlinSerial port should handle emergency parsing. +// It would not make sense to parse GCode from TMC responses, for example. +constexpr bool serial_handles_emergency(int port) { + return false + #ifdef SERIAL_PORT + || (SERIAL_PORT) == port + #endif + #ifdef SERIAL_PORT_2 + || (SERIAL_PORT_2) == port + #endif + #ifdef LCD_SERIAL_PORT + || (LCD_SERIAL_PORT) == port + #endif + ; +} + +#define DEFINE_HWSERIAL_MARLIN(name, n) \ + MSerialT name(serial_handles_emergency(n),\ + USART##n, \ + BOARD_USART##n##_TX_PIN, \ + BOARD_USART##n##_RX_PIN); \ + extern "C" void __irq_usart##n(void) { \ + my_usart_irq(USART##n->rb, USART##n->wb, USART##n##_BASE, MSerial##n); \ + } + +#define DEFINE_HWSERIAL_UART_MARLIN(name, n) \ + MSerialT name(serial_handles_emergency(n), \ + UART##n, \ + BOARD_USART##n##_TX_PIN, \ + BOARD_USART##n##_RX_PIN); \ + extern "C" void __irq_usart##n(void) { \ + my_usart_irq(UART##n->rb, UART##n->wb, UART##n##_BASE, MSerial##n); \ + } + +// Instantiate all UARTs even if they are not needed +// This avoids a bunch of logic to figure out every serial +// port which may be in use on the system. +#if DISABLED(MKS_WIFI_MODULE) + DEFINE_HWSERIAL_MARLIN(MSerial1, 1); +#endif +DEFINE_HWSERIAL_MARLIN(MSerial2, 2); +DEFINE_HWSERIAL_MARLIN(MSerial3, 3); +#if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY) + DEFINE_HWSERIAL_UART_MARLIN(MSerial4, 4); + DEFINE_HWSERIAL_UART_MARLIN(MSerial5, 5); +#endif + +// Check the type of each serial port by passing it to a template function. +// HardwareSerial is known to sometimes hang the controller when an error occurs, +// so this case will fail the static assert. All other classes are assumed to be ok. +template +constexpr bool IsSerialClassAllowed(const T&) { return true; } +constexpr bool IsSerialClassAllowed(const HardwareSerial&) { return false; } + +#define CHECK_CFG_SERIAL(A) static_assert(IsSerialClassAllowed(A), STRINGIFY(A) " is defined incorrectly"); +#define CHECK_AXIS_SERIAL(A) static_assert(IsSerialClassAllowed(A##_HARDWARE_SERIAL), STRINGIFY(A) "_HARDWARE_SERIAL must be defined in the form MSerial1, rather than Serial1"); + +// If you encounter this error, replace SerialX with MSerialX, for example MSerial3. + +// Non-TMC ports were already validated in HAL.h, so do not require verbose error messages. +#ifdef MYSERIAL1 + CHECK_CFG_SERIAL(MYSERIAL1); +#endif +#ifdef MYSERIAL2 + CHECK_CFG_SERIAL(MYSERIAL2); +#endif +#ifdef LCD_SERIAL + CHECK_CFG_SERIAL(LCD_SERIAL); +#endif +#if AXIS_HAS_HW_SERIAL(X) + CHECK_AXIS_SERIAL(X); +#endif +#if AXIS_HAS_HW_SERIAL(X2) + CHECK_AXIS_SERIAL(X2); +#endif +#if AXIS_HAS_HW_SERIAL(Y) + CHECK_AXIS_SERIAL(Y); +#endif +#if AXIS_HAS_HW_SERIAL(Y2) + CHECK_AXIS_SERIAL(Y2); +#endif +#if AXIS_HAS_HW_SERIAL(Z) + CHECK_AXIS_SERIAL(Z); +#endif +#if AXIS_HAS_HW_SERIAL(Z2) + CHECK_AXIS_SERIAL(Z2); +#endif +#if AXIS_HAS_HW_SERIAL(Z3) + CHECK_AXIS_SERIAL(Z3); +#endif +#if AXIS_HAS_HW_SERIAL(Z4) + CHECK_AXIS_SERIAL(Z4); +#endif +#if AXIS_HAS_HW_SERIAL(I) + CHECK_AXIS_SERIAL(I); +#endif +#if AXIS_HAS_HW_SERIAL(J) + CHECK_AXIS_SERIAL(J); +#endif +#if AXIS_HAS_HW_SERIAL(K) + CHECK_AXIS_SERIAL(K); +#endif +#if AXIS_HAS_HW_SERIAL(E0) + CHECK_AXIS_SERIAL(E0); +#endif +#if AXIS_HAS_HW_SERIAL(E1) + CHECK_AXIS_SERIAL(E1); +#endif +#if AXIS_HAS_HW_SERIAL(E2) + CHECK_AXIS_SERIAL(E2); +#endif +#if AXIS_HAS_HW_SERIAL(E3) + CHECK_AXIS_SERIAL(E3); +#endif +#if AXIS_HAS_HW_SERIAL(E4) + CHECK_AXIS_SERIAL(E4); +#endif +#if AXIS_HAS_HW_SERIAL(E5) + CHECK_AXIS_SERIAL(E5); +#endif +#if AXIS_HAS_HW_SERIAL(E6) + CHECK_AXIS_SERIAL(E6); +#endif +#if AXIS_HAS_HW_SERIAL(E7) + CHECK_AXIS_SERIAL(E7); +#endif + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/MarlinSerial.h b/src/HAL/STM32F1/MarlinSerial.h new file mode 100644 index 0000000..dda32fe --- /dev/null +++ b/src/HAL/STM32F1/MarlinSerial.h @@ -0,0 +1,58 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include +#include + +#include "../../inc/MarlinConfigPre.h" +#include "../../core/serial_hook.h" + +// Increase priority of serial interrupts, to reduce overflow errors +#define UART_IRQ_PRIO 1 + +struct MarlinSerial : public HardwareSerial { + MarlinSerial(struct usart_dev *usart_device, uint8 tx_pin, uint8 rx_pin) : HardwareSerial(usart_device, tx_pin, rx_pin) { } + + #ifdef UART_IRQ_PRIO + // Shadow the parent methods to set IRQ priority after begin() + void begin(uint32 baud) { + MarlinSerial::begin(baud, SERIAL_8N1); + } + + void begin(uint32 baud, uint8_t config) { + HardwareSerial::begin(baud, config); + nvic_irq_set_priority(c_dev()->irq_num, UART_IRQ_PRIO); + } + #endif +}; + +typedef Serial1Class MSerialT; + +extern MSerialT MSerial1; +extern MSerialT MSerial2; +extern MSerialT MSerial3; +#if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY) + extern MSerialT MSerial4; + extern MSerialT MSerial5; +#endif diff --git a/src/HAL/STM32F1/MinSerial.cpp b/src/HAL/STM32F1/MinSerial.cpp new file mode 100644 index 0000000..6cf68d8 --- /dev/null +++ b/src/HAL/STM32F1/MinSerial.cpp @@ -0,0 +1,117 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(POSTMORTEM_DEBUGGING) + +#include "../shared/MinSerial.h" + +#include +#include +#include + +/* Instruction Synchronization Barrier */ +#define isb() __asm__ __volatile__ ("isb" : : : "memory") + +/* Data Synchronization Barrier */ +#define dsb() __asm__ __volatile__ ("dsb" : : : "memory") + +static void TXBegin() { + #if !WITHIN(SERIAL_PORT, 1, 6) + #warning "Using POSTMORTEM_DEBUGGING requires a physical U(S)ART hardware in case of severe error." + #warning "Disabling the severe error reporting feature currently because the used serial port is not a HW port." + #else + // We use MYSERIAL1 here, so we need to figure out how to get the linked register + struct usart_dev* dev = MYSERIAL1.c_dev(); + + // Or use this if removing libmaple + // int irq = dev->irq_num; + // int nvicUART[] = { NVIC_USART1 /* = 37 */, NVIC_USART2 /* = 38 */, NVIC_USART3 /* = 39 */, NVIC_UART4 /* = 52 */, NVIC_UART5 /* = 53 */ }; + // Disabling irq means setting the bit in the NVIC ICER register located at + // Disable UART interrupt in NVIC + nvic_irq_disable(dev->irq_num); + + // Use this if removing libmaple + //SBI(NVIC_BASE->ICER[1], irq - 32); + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + dsb(); + isb(); + + rcc_clk_disable(dev->clk_id); + rcc_clk_enable(dev->clk_id); + + usart_reg_map *regs = dev->regs; + regs->CR1 = 0; // Reset the USART + regs->CR2 = 0; // 1 stop bit + + // If we don't touch the BRR (baudrate register), we don't need to recompute. Else we would need to call + usart_set_baud_rate(dev, 0, BAUDRATE); + + regs->CR1 = (USART_CR1_TE | USART_CR1_UE); // 8 bits, no parity, 1 stop bit + #endif +} + +// A SW memory barrier, to ensure GCC does not overoptimize loops +#define sw_barrier() __asm__ volatile("": : :"memory"); +static void TX(char c) { + #if WITHIN(SERIAL_PORT, 1, 6) + struct usart_dev* dev = MYSERIAL1.c_dev(); + while (!(dev->regs->SR & USART_SR_TXE)) { + hal.watchdog_refresh(); + sw_barrier(); + } + dev->regs->DR = c; + #endif +} + +void install_min_serial() { + HAL_min_serial_init = &TXBegin; + HAL_min_serial_out = &TX; +} + +#if DISABLED(DYNAMIC_VECTORTABLE) && DISABLED(STM32F0xx) // Cortex M0 can't branch to a symbol that's too far, so we have a specific hack for them +extern "C" { + __attribute__((naked)) void JumpHandler_ASM() { + __asm__ __volatile__ ( + "b CommonHandler_ASM\n" + ); + } + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __exc_hardfault(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __exc_busfault(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __exc_usagefault(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __exc_memmanage(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __exc_nmi(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __stm32reservedexception7(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __stm32reservedexception8(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __stm32reservedexception9(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __stm32reservedexception10(); + void __attribute__((naked, alias("JumpHandler_ASM"), nothrow)) __stm32reservedexception13(); +} +#endif + +#endif // POSTMORTEM_DEBUGGING +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/README.md b/src/HAL/STM32F1/README.md new file mode 100644 index 0000000..40c5633 --- /dev/null +++ b/src/HAL/STM32F1/README.md @@ -0,0 +1,11 @@ +# STM32F1 + +This HAL is for STM32F103 boards used with [Arduino STM32](https://github.com/rogerclarkmelbourne/Arduino_STM32) framework. + +Currently has been tested in Malyan M200 (103CBT6), SKRmini (103RCT6), Chitu 3d (103ZET6), and various 103VET6 boards. + +### Main developers: +- Victorpv +- xC000005 +- thisiskeithb +- tpruvot diff --git a/src/HAL/STM32F1/SPI.cpp b/src/HAL/STM32F1/SPI.cpp new file mode 100644 index 0000000..1ce2c7d --- /dev/null +++ b/src/HAL/STM32F1/SPI.cpp @@ -0,0 +1,738 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010 Perry Hung. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ + +/** + * @author Marti Bolivar + * @brief Wirish SPI implementation. + */ + +#ifdef __STM32F1__ + +#include + +#include +#include +#include + +#include +#include + +#include "../../inc/MarlinConfig.h" +#include "spi_pins.h" + +/** Time in ms for DMA receive timeout */ +#define DMA_TIMEOUT 100 + +#if CYCLES_PER_MICROSECOND != 72 + #warning "Unexpected clock speed; SPI frequency calculation will be incorrect" +#endif + +struct spi_pins { uint8_t nss, sck, miso, mosi; }; + +static const spi_pins* dev_to_spi_pins(spi_dev *dev); +static void configure_gpios(spi_dev *dev, bool as_master); +static spi_baud_rate determine_baud_rate(spi_dev *dev, uint32_t freq); + +#if BOARD_NR_SPI >= 3 && !defined(STM32_HIGH_DENSITY) + #error "The SPI library is misconfigured: 3 SPI ports only available on high density STM32 devices" +#endif + +static const spi_pins board_spi_pins[] __FLASH__ = { + #if BOARD_NR_SPI >= 1 + { BOARD_SPI1_NSS_PIN, + BOARD_SPI1_SCK_PIN, + BOARD_SPI1_MISO_PIN, + BOARD_SPI1_MOSI_PIN }, + #endif + #if BOARD_NR_SPI >= 2 + { BOARD_SPI2_NSS_PIN, + BOARD_SPI2_SCK_PIN, + BOARD_SPI2_MISO_PIN, + BOARD_SPI2_MOSI_PIN }, + #endif + #if BOARD_NR_SPI >= 3 + { BOARD_SPI3_NSS_PIN, + BOARD_SPI3_SCK_PIN, + BOARD_SPI3_MISO_PIN, + BOARD_SPI3_MOSI_PIN }, + #endif +}; + +#if BOARD_NR_SPI >= 1 + static void *_spi1_this; +#endif +#if BOARD_NR_SPI >= 2 + static void *_spi2_this; +#endif +#if BOARD_NR_SPI >= 3 + static void *_spi3_this; +#endif + +/** + * @brief Wait until TXE (tx empty) flag is set and BSY (busy) flag unset. + */ +static inline void waitSpiTxEnd(spi_dev *spi_d) { + while (spi_is_tx_empty(spi_d) == 0) { /* nada */ } // wait until TXE=1 + while (spi_is_busy(spi_d) != 0) { /* nada */ } // wait until BSY=0 +} + +/** + * Constructor + */ +SPIClass::SPIClass(uint32_t spi_num) { + _currentSetting = &_settings[spi_num - 1]; // SPI channels are called 1 2 and 3 but the array is zero indexed + + switch (spi_num) { + #if BOARD_NR_SPI >= 1 + case 1: + _currentSetting->spi_d = SPI1; + _spi1_this = (void*)this; + break; + #endif + #if BOARD_NR_SPI >= 2 + case 2: + _currentSetting->spi_d = SPI2; + _spi2_this = (void*)this; + break; + #endif + #if BOARD_NR_SPI >= 3 + case 3: + _currentSetting->spi_d = SPI3; + _spi3_this = (void*)this; + break; + #endif + default: ASSERT(0); + } + + // Init things specific to each SPI device + // clock divider setup is a bit of hack, and needs to be improved at a later date. + #if BOARD_NR_SPI >= 1 + _settings[0].spi_d = SPI1; + _settings[0].clockDivider = determine_baud_rate(_settings[0].spi_d, _settings[0].clock); + _settings[0].spiDmaDev = DMA1; + _settings[0].spiTxDmaChannel = DMA_CH3; + _settings[0].spiRxDmaChannel = DMA_CH2; + #endif + #if BOARD_NR_SPI >= 2 + _settings[1].spi_d = SPI2; + _settings[1].clockDivider = determine_baud_rate(_settings[1].spi_d, _settings[1].clock); + _settings[1].spiDmaDev = DMA1; + _settings[1].spiTxDmaChannel = DMA_CH5; + _settings[1].spiRxDmaChannel = DMA_CH4; + #endif + #if BOARD_NR_SPI >= 3 + _settings[2].spi_d = SPI3; + _settings[2].clockDivider = determine_baud_rate(_settings[2].spi_d, _settings[2].clock); + _settings[2].spiDmaDev = DMA2; + _settings[2].spiTxDmaChannel = DMA_CH2; + _settings[2].spiRxDmaChannel = DMA_CH1; + #endif + + // added for DMA callbacks. + _currentSetting->state = SPI_STATE_IDLE; +} + +SPIClass::SPIClass(int8_t mosi, int8_t miso, int8_t sclk, int8_t ssel) : SPIClass(1) { + #if BOARD_NR_SPI >= 1 + if (mosi == BOARD_SPI1_MOSI_PIN) setModule(1); + #endif + #if BOARD_NR_SPI >= 2 + if (mosi == BOARD_SPI2_MOSI_PIN) setModule(2); + #endif + #if BOARD_NR_SPI >= 3 + if (mosi == BOARD_SPI3_MOSI_PIN) setModule(3); + #endif +} + +/** + * Set up/tear down + */ +void SPIClass::updateSettings() { + uint32_t flags = ((_currentSetting->bitOrder == MSBFIRST ? SPI_FRAME_MSB : SPI_FRAME_LSB) | _currentSetting->dataSize | SPI_SW_SLAVE | SPI_SOFT_SS); + spi_master_enable(_currentSetting->spi_d, (spi_baud_rate)_currentSetting->clockDivider, (spi_mode)_currentSetting->dataMode, flags); +} + +void SPIClass::begin() { + spi_init(_currentSetting->spi_d); + configure_gpios(_currentSetting->spi_d, 1); + updateSettings(); + // added for DMA callbacks. + _currentSetting->state = SPI_STATE_READY; +} + +void SPIClass::beginSlave() { + spi_init(_currentSetting->spi_d); + configure_gpios(_currentSetting->spi_d, 0); + uint32_t flags = ((_currentSetting->bitOrder == MSBFIRST ? SPI_FRAME_MSB : SPI_FRAME_LSB) | _currentSetting->dataSize); + spi_slave_enable(_currentSetting->spi_d, (spi_mode)_currentSetting->dataMode, flags); + // added for DMA callbacks. + _currentSetting->state = SPI_STATE_READY; +} + +void SPIClass::end() { + if (!spi_is_enabled(_currentSetting->spi_d)) return; + + // Follows RM0008's sequence for disabling a SPI in master/slave + // full duplex mode. + while (spi_is_rx_nonempty(_currentSetting->spi_d)) { + // FIXME [0.1.0] remove this once you have an interrupt based driver + volatile uint16_t rx __attribute__((unused)) = spi_rx_reg(_currentSetting->spi_d); + } + waitSpiTxEnd(_currentSetting->spi_d); + + spi_peripheral_disable(_currentSetting->spi_d); + // added for DMA callbacks. + // Need to add unsetting the callbacks for the DMA channels. + _currentSetting->state = SPI_STATE_IDLE; +} + +/* Roger Clark added 3 functions */ +void SPIClass::setClockDivider(uint32_t clockDivider) { + _currentSetting->clockDivider = clockDivider; + uint32_t cr1 = _currentSetting->spi_d->regs->CR1 & ~(SPI_CR1_BR); + _currentSetting->spi_d->regs->CR1 = cr1 | (clockDivider & SPI_CR1_BR); +} + +void SPIClass::setBitOrder(BitOrder bitOrder) { + _currentSetting->bitOrder = bitOrder; + uint32_t cr1 = _currentSetting->spi_d->regs->CR1 & ~(SPI_CR1_LSBFIRST); + if (bitOrder == LSBFIRST) cr1 |= SPI_CR1_LSBFIRST; + _currentSetting->spi_d->regs->CR1 = cr1; +} + +/** + * Victor Perez. Added to test changing datasize from 8 to 16 bit modes on the fly. + * Input parameter should be SPI_CR1_DFF set to 0 or 1 on a 32bit word. + */ +void SPIClass::setDataSize(uint32_t datasize) { + _currentSetting->dataSize = datasize; + uint32_t cr1 = _currentSetting->spi_d->regs->CR1 & ~(SPI_CR1_DFF); + uint8_t en = spi_is_enabled(_currentSetting->spi_d); + spi_peripheral_disable(_currentSetting->spi_d); + _currentSetting->spi_d->regs->CR1 = cr1 | (datasize & SPI_CR1_DFF) | en; +} + +void SPIClass::setDataMode(uint8_t dataMode) { + /** + * Notes: + * As far as we know the AVR numbers for dataMode match the numbers required by the STM32. + * From the AVR doc https://www.atmel.com/images/doc2585.pdf section 2.4 + * + * SPI Mode CPOL CPHA Shift SCK-edge Capture SCK-edge + * 0 0 0 Falling Rising + * 1 0 1 Rising Falling + * 2 1 0 Rising Falling + * 3 1 1 Falling Rising + * + * On the STM32 it appears to be + * + * bit 1 - CPOL : Clock polarity + * (This bit should not be changed when communication is ongoing) + * 0 : CLK to 0 when idle + * 1 : CLK to 1 when idle + * + * bit 0 - CPHA : Clock phase + * (This bit should not be changed when communication is ongoing) + * 0 : The first clock transition is the first data capture edge + * 1 : The second clock transition is the first data capture edge + * + * If someone finds this is not the case or sees a logic error with this let me know ;-) + */ + _currentSetting->dataMode = dataMode; + uint32_t cr1 = _currentSetting->spi_d->regs->CR1 & ~(SPI_CR1_CPOL|SPI_CR1_CPHA); + _currentSetting->spi_d->regs->CR1 = cr1 | (dataMode & (SPI_CR1_CPOL|SPI_CR1_CPHA)); +} + +void SPIClass::beginTransaction(uint8_t pin, const SPISettings &settings) { + setBitOrder(settings.bitOrder); + setDataMode(settings.dataMode); + setDataSize(settings.dataSize); + setClockDivider(determine_baud_rate(_currentSetting->spi_d, settings.clock)); + begin(); +} + +void SPIClass::beginTransactionSlave(const SPISettings &settings) { + setBitOrder(settings.bitOrder); + setDataMode(settings.dataMode); + setDataSize(settings.dataSize); + beginSlave(); +} + +void SPIClass::endTransaction() { } + +/** + * I/O + */ + +uint16_t SPIClass::read() { + while (!spi_is_rx_nonempty(_currentSetting->spi_d)) { /* nada */ } + return (uint16_t)spi_rx_reg(_currentSetting->spi_d); +} + +void SPIClass::read(uint8_t *buf, uint32_t len) { + if (len == 0) return; + spi_rx_reg(_currentSetting->spi_d); // clear the RX buffer in case a byte is waiting on it. + spi_reg_map * regs = _currentSetting->spi_d->regs; + // start sequence: write byte 0 + regs->DR = 0x00FF; // write the first byte + // main loop + while (--len) { + while (!(regs->SR & SPI_SR_TXE)) { /* nada */ } // wait for TXE flag + noInterrupts(); // go atomic level - avoid interrupts to surely get the previously received data + regs->DR = 0x00FF; // write the next data item to be transmitted into the SPI_DR register. This clears the TXE flag. + while (!(regs->SR & SPI_SR_RXNE)) { /* nada */ } // wait till data is available in the DR register + *buf++ = (uint8)(regs->DR); // read and store the received byte. This clears the RXNE flag. + interrupts(); // let systick do its job + } + // read remaining last byte + while (!(regs->SR & SPI_SR_RXNE)) { /* nada */ } // wait till data is available in the Rx register + *buf++ = (uint8)(regs->DR); // read and store the received byte +} + +void SPIClass::write(uint16_t data) { + /* Added for 16bit data Victor Perez. Roger Clark + * Improved speed by just directly writing the single byte to the SPI data reg and wait for completion, + * by taking the Tx code from transfer(byte) + * This almost doubles the speed of this function. + */ + spi_tx_reg(_currentSetting->spi_d, data); // write the data to be transmitted into the SPI_DR register (this clears the TXE flag) + waitSpiTxEnd(_currentSetting->spi_d); +} + +void SPIClass::write16(uint16_t data) { + // Added by stevestrong: write two consecutive bytes in 8 bit mode (DFF=0) + spi_tx_reg(_currentSetting->spi_d, data>>8); // write high byte + while (!spi_is_tx_empty(_currentSetting->spi_d)) { /* nada */ } // Wait until TXE=1 + spi_tx_reg(_currentSetting->spi_d, data); // write low byte + waitSpiTxEnd(_currentSetting->spi_d); +} + +void SPIClass::write(uint16_t data, uint32_t n) { + // Added by stevstrong: Repeatedly send same data by the specified number of times + spi_reg_map * regs = _currentSetting->spi_d->regs; + while (n--) { + regs->DR = data; // write the data to be transmitted into the SPI_DR register (this clears the TXE flag) + while (!(regs->SR & SPI_SR_TXE)) { /* nada */ } // wait till Tx empty + } + while (regs->SR & SPI_SR_BSY) { /* nada */ } // wait until BSY=0 before returning +} + +void SPIClass::write(const void *data, uint32_t length) { + spi_dev * spi_d = _currentSetting->spi_d; + spi_tx(spi_d, data, length); // data can be array of bytes or words + waitSpiTxEnd(spi_d); +} + +uint8_t SPIClass::transfer(uint8_t byte) const { + spi_dev * spi_d = _currentSetting->spi_d; + spi_rx_reg(spi_d); // read any previous data + spi_tx_reg(spi_d, byte); // Write the data item to be transmitted into the SPI_DR register + waitSpiTxEnd(spi_d); + return (uint8)spi_rx_reg(spi_d); // "... and read the last received data." +} + +uint16_t SPIClass::transfer16(uint16_t data) const { + // Modified by stevestrong: write & read two consecutive bytes in 8 bit mode (DFF=0) + // This is more effective than two distinct byte transfers + spi_dev * spi_d = _currentSetting->spi_d; + spi_rx_reg(spi_d); // read any previous data + spi_tx_reg(spi_d, data>>8); // write high byte + waitSpiTxEnd(spi_d); // wait until TXE=1 and then wait until BSY=0 + uint16_t ret = spi_rx_reg(spi_d)<<8; // read and shift high byte + spi_tx_reg(spi_d, data); // write low byte + waitSpiTxEnd(spi_d); // wait until TXE=1 and then wait until BSY=0 + ret += spi_rx_reg(spi_d); // read low byte + return ret; +} + +/** + * Roger Clark and Victor Perez, 2015 + * Performs a DMA SPI transfer with at least a receive buffer. + * If a TX buffer is not provided, FF is sent over and over for the length of the transfer. + * On exit TX buffer is not modified, and RX buffer contains the received data. + * Still in progress. + */ +void SPIClass::dmaTransferSet(const void *transmitBuf, void *receiveBuf) { + dma_init(_currentSetting->spiDmaDev); + //spi_rx_dma_enable(_currentSetting->spi_d); + //spi_tx_dma_enable(_currentSetting->spi_d); + dma_xfer_size dma_bit_size = (_currentSetting->dataSize==DATA_SIZE_16BIT) ? DMA_SIZE_16BITS : DMA_SIZE_8BITS; + dma_setup_transfer(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, &_currentSetting->spi_d->regs->DR, + dma_bit_size, receiveBuf, dma_bit_size, (DMA_MINC_MODE | DMA_TRNS_CMPLT ));// receive buffer DMA + if (!transmitBuf) { + transmitBuf = &ff; + dma_setup_transfer(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &_currentSetting->spi_d->regs->DR, + dma_bit_size, (volatile void*)transmitBuf, dma_bit_size, (DMA_FROM_MEM));// Transmit FF repeatedly + } + else { + dma_setup_transfer(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &_currentSetting->spi_d->regs->DR, + dma_bit_size, (volatile void*)transmitBuf, dma_bit_size, (DMA_MINC_MODE | DMA_FROM_MEM ));// Transmit buffer DMA + } + dma_set_priority(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, DMA_PRIORITY_LOW); + dma_set_priority(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, DMA_PRIORITY_VERY_HIGH); +} + +uint8_t SPIClass::dmaTransferRepeat(uint16_t length) { + if (length == 0) return 0; + if (spi_is_rx_nonempty(_currentSetting->spi_d) == 1) spi_rx_reg(_currentSetting->spi_d); + _currentSetting->state = SPI_STATE_TRANSFER; + dma_set_num_transfers(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, length); + dma_set_num_transfers(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, length); + dma_enable(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel);// enable receive + dma_enable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel);// enable transmit + spi_rx_dma_enable(_currentSetting->spi_d); + spi_tx_dma_enable(_currentSetting->spi_d); + if (_currentSetting->receiveCallback) + return 0; + + //uint32_t m = millis(); + uint8_t b = 0; + uint32_t m = millis(); + while (!(dma_get_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel) & DMA_ISR_TCIF1)) { + // Avoid interrupts and just loop waiting for the flag to be set. + if ((millis() - m) > DMA_TIMEOUT) { b = 2; break; } + } + + waitSpiTxEnd(_currentSetting->spi_d); // until TXE=1 and BSY=0 + spi_tx_dma_disable(_currentSetting->spi_d); + spi_rx_dma_disable(_currentSetting->spi_d); + dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel); + dma_clear_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel); + dma_clear_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + _currentSetting->state = SPI_STATE_READY; + return b; +} + +/** + * Roger Clark and Victor Perez, 2015 + * Performs a DMA SPI transfer with at least a receive buffer. + * If a TX buffer is not provided, FF is sent over and over for the length of the transfer. + * On exit TX buffer is not modified, and RX buffer contains the received data. + * Still in progress. + */ +uint8_t SPIClass::dmaTransfer(const void *transmitBuf, void *receiveBuf, uint16_t length) { + dmaTransferSet(transmitBuf, receiveBuf); + return dmaTransferRepeat(length); +} + +/** + * Roger Clark and Victor Perez, 2015 + * Performs a DMA SPI send using a TX buffer. + * On exit TX buffer is not modified. + * Still in progress. + * 2016 - stevstrong - reworked to automatically detect bit size from SPI setting + */ +void SPIClass::dmaSendSet(const void * transmitBuf, bool minc) { + uint32_t flags = ( (DMA_MINC_MODE*minc) | DMA_FROM_MEM | DMA_TRNS_CMPLT); + dma_init(_currentSetting->spiDmaDev); + dma_xfer_size dma_bit_size = (_currentSetting->dataSize==DATA_SIZE_16BIT) ? DMA_SIZE_16BITS : DMA_SIZE_8BITS; + dma_setup_transfer(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &_currentSetting->spi_d->regs->DR, dma_bit_size, + (volatile void*)transmitBuf, dma_bit_size, flags);// Transmit buffer DMA + dma_set_priority(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, DMA_PRIORITY_LOW); +} + +uint8_t SPIClass::dmaSendRepeat(uint16_t length) { + if (length == 0) return 0; + + dma_clear_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + dma_set_num_transfers(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, length); + _currentSetting->state = SPI_STATE_TRANSMIT; + dma_enable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); // enable transmit + spi_tx_dma_enable(_currentSetting->spi_d); + if (_currentSetting->transmitCallback) return 0; + + uint32_t m = millis(); + uint8_t b = 0; + while (!(dma_get_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel) & DMA_ISR_TCIF1)) { + // Avoid interrupts and just loop waiting for the flag to be set. + if ((millis() - m) > DMA_TIMEOUT) { b = 2; break; } + } + waitSpiTxEnd(_currentSetting->spi_d); // until TXE=1 and BSY=0 + spi_tx_dma_disable(_currentSetting->spi_d); + dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + dma_clear_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + _currentSetting->state = SPI_STATE_READY; + return b; +} + +uint8_t SPIClass::dmaSend(const void * transmitBuf, uint16_t length, bool minc) { + dmaSendSet(transmitBuf, minc); + return dmaSendRepeat(length); +} + +uint8_t SPIClass::dmaSendAsync(const void * transmitBuf, uint16_t length, bool minc) { + uint8_t b = 0; + + if (_currentSetting->state != SPI_STATE_READY) { + uint32_t m = millis(); + while (!(dma_get_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel) & DMA_ISR_TCIF1)) { + //Avoid interrupts and just loop waiting for the flag to be set. + //delayMicroseconds(10); + if ((millis() - m) > DMA_TIMEOUT) { b = 2; break; } + } + waitSpiTxEnd(_currentSetting->spi_d); // until TXE=1 and BSY=0 + spi_tx_dma_disable(_currentSetting->spi_d); + dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + _currentSetting->state = SPI_STATE_READY; + } + + if (length == 0) return 0; + uint32_t flags = ( (DMA_MINC_MODE*minc) | DMA_FROM_MEM | DMA_TRNS_CMPLT); + + dma_init(_currentSetting->spiDmaDev); + // TX + dma_xfer_size dma_bit_size = (_currentSetting->dataSize==DATA_SIZE_16BIT) ? DMA_SIZE_16BITS : DMA_SIZE_8BITS; + dma_setup_transfer(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &_currentSetting->spi_d->regs->DR, + dma_bit_size, (volatile void*)transmitBuf, dma_bit_size, flags);// Transmit buffer DMA + dma_set_num_transfers(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, length); + dma_clear_isr_bits(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + dma_enable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel);// enable transmit + spi_tx_dma_enable(_currentSetting->spi_d); + + _currentSetting->state = SPI_STATE_TRANSMIT; + return b; +} + + +/** + * New functions added to manage callbacks. + * Victor Perez 2017 + */ +void SPIClass::onReceive(void(*callback)()) { + _currentSetting->receiveCallback = callback; + if (callback) { + switch (_currentSetting->spi_d->clk_id) { + #if BOARD_NR_SPI >= 1 + case RCC_SPI1: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, &SPIClass::_spi1EventCallback); + break; + #endif + #if BOARD_NR_SPI >= 2 + case RCC_SPI2: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, &SPIClass::_spi2EventCallback); + break; + #endif + #if BOARD_NR_SPI >= 3 + case RCC_SPI3: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel, &SPIClass::_spi3EventCallback); + break; + #endif + default: + ASSERT(0); + } + } + else { + dma_detach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel); + } +} + +void SPIClass::onTransmit(void(*callback)()) { + _currentSetting->transmitCallback = callback; + if (callback) { + switch (_currentSetting->spi_d->clk_id) { + #if BOARD_NR_SPI >= 1 + case RCC_SPI1: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &SPIClass::_spi1EventCallback); + break; + #endif + #if BOARD_NR_SPI >= 2 + case RCC_SPI2: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &SPIClass::_spi2EventCallback); + break; + #endif + #if BOARD_NR_SPI >= 3 + case RCC_SPI3: + dma_attach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel, &SPIClass::_spi3EventCallback); + break; + #endif + default: + ASSERT(0); + } + } + else { + dma_detach_interrupt(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + } +} + +/** + * TODO: check if better to first call the customer code, next disable the DMA requests. + * Also see if we need to check whether callbacks are set or not, may be better to be checked + * during the initial setup and only set the callback to EventCallback if they are set. + */ +void SPIClass::EventCallback() { + waitSpiTxEnd(_currentSetting->spi_d); + switch (_currentSetting->state) { + case SPI_STATE_TRANSFER: + while (spi_is_rx_nonempty(_currentSetting->spi_d)) { /* nada */ } + _currentSetting->state = SPI_STATE_READY; + spi_tx_dma_disable(_currentSetting->spi_d); + spi_rx_dma_disable(_currentSetting->spi_d); + //dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + //dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiRxDmaChannel); + if (_currentSetting->receiveCallback) + _currentSetting->receiveCallback(); + break; + case SPI_STATE_TRANSMIT: + _currentSetting->state = SPI_STATE_READY; + spi_tx_dma_disable(_currentSetting->spi_d); + //dma_disable(_currentSetting->spiDmaDev, _currentSetting->spiTxDmaChannel); + if (_currentSetting->transmitCallback) + _currentSetting->transmitCallback(); + break; + default: + break; + } +} + +void SPIClass::attachInterrupt() { + // Should be enableInterrupt() +} + +void SPIClass::detachInterrupt() { + // Should be disableInterrupt() +} + +/** + * Pin accessors + */ + +uint8_t SPIClass::misoPin() { + return dev_to_spi_pins(_currentSetting->spi_d)->miso; +} + +uint8_t SPIClass::mosiPin() { + return dev_to_spi_pins(_currentSetting->spi_d)->mosi; +} + +uint8_t SPIClass::sckPin() { + return dev_to_spi_pins(_currentSetting->spi_d)->sck; +} + +uint8_t SPIClass::nssPin() { + return dev_to_spi_pins(_currentSetting->spi_d)->nss; +} + +/** + * Deprecated functions + */ +uint8_t SPIClass::send(uint8_t data) { write(data); return 1; } +uint8_t SPIClass::send(uint8_t *buf, uint32_t len) { write(buf, len); return len; } +uint8_t SPIClass::recv() { return read(); } + +/** + * DMA call back functions, one per port. + */ +#if BOARD_NR_SPI >= 1 + void SPIClass::_spi1EventCallback() { + reinterpret_cast(_spi1_this)->EventCallback(); + } +#endif +#if BOARD_NR_SPI >= 2 + void SPIClass::_spi2EventCallback() { + reinterpret_cast(_spi2_this)->EventCallback(); + } +#endif +#if BOARD_NR_SPI >= 3 + void SPIClass::_spi3EventCallback() { + reinterpret_cast(_spi3_this)->EventCallback(); + } +#endif + +/** + * Auxiliary functions + */ +static const spi_pins* dev_to_spi_pins(spi_dev *dev) { + switch (dev->clk_id) { + #if BOARD_NR_SPI >= 1 + case RCC_SPI1: return board_spi_pins; + #endif + #if BOARD_NR_SPI >= 2 + case RCC_SPI2: return board_spi_pins + 1; + #endif + #if BOARD_NR_SPI >= 3 + case RCC_SPI3: return board_spi_pins + 2; + #endif + default: return nullptr; + } +} + +static void disable_pwm(const stm32_pin_info *i) { + if (i->timer_device) + timer_set_mode(i->timer_device, i->timer_channel, TIMER_DISABLED); +} + +static void configure_gpios(spi_dev *dev, bool as_master) { + const spi_pins *pins = dev_to_spi_pins(dev); + if (!pins) return; + + const stm32_pin_info *nssi = &PIN_MAP[pins->nss], + *scki = &PIN_MAP[pins->sck], + *misoi = &PIN_MAP[pins->miso], + *mosii = &PIN_MAP[pins->mosi]; + + disable_pwm(nssi); + disable_pwm(scki); + disable_pwm(misoi); + disable_pwm(mosii); + + spi_config_gpios(dev, as_master, nssi->gpio_device, nssi->gpio_bit, + scki->gpio_device, scki->gpio_bit, misoi->gpio_bit, + mosii->gpio_bit); +} + +static const spi_baud_rate baud_rates[8] __FLASH__ = { + SPI_BAUD_PCLK_DIV_2, + SPI_BAUD_PCLK_DIV_4, + SPI_BAUD_PCLK_DIV_8, + SPI_BAUD_PCLK_DIV_16, + SPI_BAUD_PCLK_DIV_32, + SPI_BAUD_PCLK_DIV_64, + SPI_BAUD_PCLK_DIV_128, + SPI_BAUD_PCLK_DIV_256, +}; + +/** + * Note: This assumes you're on a LeafLabs-style board + * (CYCLES_PER_MICROSECOND == 72, APB2 at 72MHz, APB1 at 36MHz). + */ +static spi_baud_rate determine_baud_rate(spi_dev *dev, uint32_t freq) { + uint32_t clock = 0; + switch (rcc_dev_clk(dev->clk_id)) { + case RCC_AHB: + case RCC_APB2: clock = STM32_PCLK2; break; // 72 Mhz + case RCC_APB1: clock = STM32_PCLK1; break; // 36 Mhz + } + clock >>= 1; + + uint8_t i = 0; + while (i < 7 && freq < clock) { clock >>= 1; i++; } + return baud_rates[i]; +} + +SPIClass SPI(SPI_DEVICE); + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/SPI.h b/src/HAL/STM32F1/SPI.h new file mode 100644 index 0000000..13f4d5e --- /dev/null +++ b/src/HAL/STM32F1/SPI.h @@ -0,0 +1,417 @@ +/****************************************************************************** + * The MIT License + * + * Copyright (c) 2010 Perry Hung. + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, copy, + * modify, merge, publish, distribute, sublicense, and/or sell copies + * of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + *****************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include +#include + +// SPI_HAS_TRANSACTION means SPI has +// - beginTransaction() +// - endTransaction() +// - usingInterrupt() +// - SPISetting(clock, bitOrder, dataMode) +//#define SPI_HAS_TRANSACTION + +#define SPI_CLOCK_DIV2 SPI_BAUD_PCLK_DIV_2 +#define SPI_CLOCK_DIV4 SPI_BAUD_PCLK_DIV_4 +#define SPI_CLOCK_DIV8 SPI_BAUD_PCLK_DIV_8 +#define SPI_CLOCK_DIV16 SPI_BAUD_PCLK_DIV_16 +#define SPI_CLOCK_DIV32 SPI_BAUD_PCLK_DIV_32 +#define SPI_CLOCK_DIV64 SPI_BAUD_PCLK_DIV_64 +#define SPI_CLOCK_DIV128 SPI_BAUD_PCLK_DIV_128 +#define SPI_CLOCK_DIV256 SPI_BAUD_PCLK_DIV_256 + +/* + * Roger Clark. 20150106 + * Commented out redundant AVR defined + * +#define SPI_MODE_MASK 0x0C // CPOL = bit 3, CPHA = bit 2 on SPCR +#define SPI_CLOCK_MASK 0x03 // SPR1 = bit 1, SPR0 = bit 0 on SPCR +#define SPI_2XCLOCK_MASK 0x01 // SPI2X = bit 0 on SPSR + +// define SPI_AVR_EIMSK for AVR boards with external interrupt pins +#ifdef EIMSK + #define SPI_AVR_EIMSK EIMSK +#elif defined(GICR) + #define SPI_AVR_EIMSK GICR +#elif defined(GIMSK) + #define SPI_AVR_EIMSK GIMSK +#endif +*/ + +#ifndef STM32_LSBFIRST + #define STM32_LSBFIRST 0 +#endif +#ifndef STM32_MSBFIRST + #define STM32_MSBFIRST 1 +#endif + +// PC13 or PA4 +#define BOARD_SPI_DEFAULT_SS PA4 +//#define BOARD_SPI_DEFAULT_SS PC13 + +#define SPI_MODE0 SPI_MODE_0 +#define SPI_MODE1 SPI_MODE_1 +#define SPI_MODE2 SPI_MODE_2 +#define SPI_MODE3 SPI_MODE_3 + +#define DATA_SIZE_8BIT SPI_CR1_DFF_8_BIT +#define DATA_SIZE_16BIT SPI_CR1_DFF_16_BIT + +typedef enum { + SPI_STATE_IDLE, + SPI_STATE_READY, + SPI_STATE_RECEIVE, + SPI_STATE_TRANSMIT, + SPI_STATE_TRANSFER +} spi_mode_t; + +class SPISettings { +public: + SPISettings(uint32_t inClock, BitOrder inBitOrder, uint8_t inDataMode) { + if (__builtin_constant_p(inClock)) + init_AlwaysInline(inClock, inBitOrder, inDataMode, DATA_SIZE_8BIT); + else + init_MightInline(inClock, inBitOrder, inDataMode, DATA_SIZE_8BIT); + } + SPISettings(uint32_t inClock, BitOrder inBitOrder, uint8_t inDataMode, uint32_t inDataSize) { + if (__builtin_constant_p(inClock)) + init_AlwaysInline(inClock, inBitOrder, inDataMode, inDataSize); + else + init_MightInline(inClock, inBitOrder, inDataMode, inDataSize); + } + SPISettings(uint32_t inClock) { + if (__builtin_constant_p(inClock)) + init_AlwaysInline(inClock, MSBFIRST, SPI_MODE0, DATA_SIZE_8BIT); + else + init_MightInline(inClock, MSBFIRST, SPI_MODE0, DATA_SIZE_8BIT); + } + SPISettings() { + init_AlwaysInline(4000000, MSBFIRST, SPI_MODE0, DATA_SIZE_8BIT); + } +private: + void init_MightInline(uint32_t inClock, BitOrder inBitOrder, uint8_t inDataMode, uint32_t inDataSize) { + init_AlwaysInline(inClock, inBitOrder, inDataMode, inDataSize); + } + void init_AlwaysInline(uint32_t inClock, BitOrder inBitOrder, uint8_t inDataMode, uint32_t inDataSize) __attribute__((__always_inline__)) { + clock = inClock; + bitOrder = inBitOrder; + dataMode = inDataMode; + dataSize = inDataSize; + //state = SPI_STATE_IDLE; + } + uint32_t clock; + uint32_t dataSize; + uint32_t clockDivider; + BitOrder bitOrder; + uint8_t dataMode; + uint8_t _SSPin; + volatile spi_mode_t state; + spi_dev *spi_d; + dma_channel spiRxDmaChannel, spiTxDmaChannel; + dma_dev* spiDmaDev; + void (*receiveCallback)() = nullptr; + void (*transmitCallback)() = nullptr; + + friend class SPIClass; +}; + +/* + * Kept for compat. + */ +static const uint8_t ff = 0xFF; + +/** + * @brief Wirish SPI interface. + * + * This implementation uses software slave management, so the caller + * is responsible for controlling the slave select line. + */ +class SPIClass { + +public: + /** + * @param spiPortNumber Number of the SPI port to manage. + */ + SPIClass(uint32_t spiPortNumber); + + /** + * Init using pins + */ + SPIClass(int8_t mosi, int8_t miso, int8_t sclk, int8_t ssel=-1); + + /** + * @brief Equivalent to begin(SPI_1_125MHZ, MSBFIRST, 0). + */ + void begin(); + + /** + * @brief Turn on a SPI port and set its GPIO pin modes for use as a slave. + * + * SPI port is enabled in full duplex mode, with software slave management. + * + * @param bitOrder Either LSBFIRST (little-endian) or MSBFIRST(big-endian) + * @param mode SPI mode to use + */ + void beginSlave(uint32_t bitOrder, uint32_t mode); + + /** + * @brief Equivalent to beginSlave(MSBFIRST, 0). + */ + void beginSlave(); + + /** + * @brief Disables the SPI port, but leaves its GPIO pin modes unchanged. + */ + void end(); + + void beginTransaction(const SPISettings &settings) { beginTransaction(BOARD_SPI_DEFAULT_SS, settings); } + void beginTransaction(uint8_t pin, const SPISettings &settings); + void endTransaction(); + + void beginTransactionSlave(const SPISettings &settings); + + void setClockDivider(uint32_t clockDivider); + void setBitOrder(BitOrder bitOrder); + void setDataMode(uint8_t dataMode); + + // SPI Configuration methods + void attachInterrupt(); + void detachInterrupt(); + + /* Victor Perez. Added to change datasize from 8 to 16 bit modes on the fly. + * Input parameter should be SPI_CR1_DFF set to 0 or 1 on a 32bit word. + * Requires an added function spi_data_size on STM32F1 / cores / maple / libmaple / spi.c + */ + void setDataSize(uint32_t ds); + + uint32_t getDataSize() { return _currentSetting->dataSize; } + + /* Victor Perez 2017. Added to set and clear callback functions for callback + * on DMA transfer completion. + * onReceive used to set the callback in case of dmaTransfer (tx/rx), once rx is completed + * onTransmit used to set the callback in case of dmaSend (tx only). That function + * will NOT be called in case of TX/RX + */ + void onReceive(void(*)()); + void onTransmit(void(*)()); + + /* + * I/O + */ + + /** + * @brief Return the next unread byte/word. + * + * If there is no unread byte/word waiting, this function will block + * until one is received. + */ + uint16_t read(); + + /** + * @brief Read length bytes, storing them into buffer. + * @param buffer Buffer to store received bytes into. + * @param length Number of bytes to store in buffer. This + * function will block until the desired number of + * bytes have been read. + */ + void read(uint8_t *buffer, uint32_t length); + + /** + * @brief Transmit one byte/word. + * @param data to transmit. + */ + void write(uint16_t data); + void write16(uint16_t data); // write 2 bytes in 8 bit mode (DFF=0) + + /** + * @brief Transmit one byte/word a specified number of times. + * @param data to transmit. + */ + void write(uint16_t data, uint32_t n); + + /** + * @brief Transmit multiple bytes/words. + * @param buffer Bytes/words to transmit. + * @param length Number of bytes/words in buffer to transmit. + */ + void write(const void * buffer, uint32_t length); + + /** + * @brief Transmit a byte, then return the next unread byte. + * + * This function transmits before receiving. + * + * @param data Byte to transmit. + * @return Next unread byte. + */ + uint8_t transfer(uint8_t data) const; + uint16_t transfer16(uint16_t data) const; + + /** + * @brief Sets up a DMA Transfer for "length" bytes. + * The transfer mode (8 or 16 bit mode) is evaluated from the SPI peripheral setting. + * + * This function transmits and receives to buffers. + * + * @param transmitBuf buffer Bytes to transmit. If passed as 0, it sends FF repeatedly for "length" bytes + * @param receiveBuf buffer Bytes to save received data. + * @param length Number of bytes in buffer to transmit. + */ + uint8_t dmaTransfer(const void * transmitBuf, void * receiveBuf, uint16_t length); + void dmaTransferSet(const void *transmitBuf, void *receiveBuf); + uint8_t dmaTransferRepeat(uint16_t length); + + /** + * @brief Sets up a DMA Transmit for SPI 8 or 16 bit transfer mode. + * The transfer mode (8 or 16 bit mode) is evaluated from the SPI peripheral setting. + * + * This function only transmits and does not care about the RX fifo. + * + * @param data buffer half words to transmit, + * @param length Number of bytes in buffer to transmit. + * @param minc Set to use Memory Increment mode, clear to use Circular mode. + */ + uint8_t dmaSend(const void * transmitBuf, uint16_t length, bool minc = 1); + void dmaSendSet(const void * transmitBuf, bool minc); + uint8_t dmaSendRepeat(uint16_t length); + + uint8_t dmaSendAsync(const void * transmitBuf, uint16_t length, bool minc = 1); + /* + * Pin accessors + */ + + /** + * @brief Return the number of the MISO (master in, slave out) pin + */ + uint8_t misoPin(); + + /** + * @brief Return the number of the MOSI (master out, slave in) pin + */ + uint8_t mosiPin(); + + /** + * @brief Return the number of the SCK (serial clock) pin + */ + uint8_t sckPin(); + + /** + * @brief Return the number of the NSS (slave select) pin + */ + uint8_t nssPin(); + + /* Escape hatch */ + + /** + * @brief Get a pointer to the underlying libmaple spi_dev for + * this HardwareSPI instance. + */ + spi_dev* c_dev() { return _currentSetting->spi_d; } + + spi_dev* dev() { return _currentSetting->spi_d; } + + /** + * @brief Sets the number of the SPI peripheral to be used by + * this HardwareSPI instance. + * + * @param spi_num Number of the SPI port. 1-2 in low density devices + * or 1-3 in high density devices. + */ + void setModule(int spi_num) { + _currentSetting = &_settings[spi_num - 1];// SPI channels are called 1 2 and 3 but the array is zero indexed + } + + /* -- The following methods are deprecated --------------------------- */ + + /** + * @brief Deprecated. + * + * Use HardwareSPI::transfer() instead. + * + * @see HardwareSPI::transfer() + */ + uint8_t send(uint8_t data); + + /** + * @brief Deprecated. + * + * Use HardwareSPI::write() in combination with + * HardwareSPI::read() (or HardwareSPI::transfer()) instead. + * + * @see HardwareSPI::write() + * @see HardwareSPI::read() + * @see HardwareSPI::transfer() + */ + uint8_t send(uint8_t *data, uint32_t length); + + /** + * @brief Deprecated. + * + * Use HardwareSPI::read() instead. + * + * @see HardwareSPI::read() + */ + uint8_t recv(); + +private: + + SPISettings _settings[BOARD_NR_SPI]; + SPISettings *_currentSetting; + + void updateSettings(); + + /* + * Functions added for DMA transfers with Callback. + * Experimental. + */ + + void EventCallback(); + + #if BOARD_NR_SPI >= 1 + static void _spi1EventCallback(); + #endif + #if BOARD_NR_SPI >= 2 + static void _spi2EventCallback(); + #endif + #if BOARD_NR_SPI >= 3 + static void _spi3EventCallback(); + #endif + /* + spi_dev *spi_d; + uint8_t _SSPin; + uint32_t clockDivider; + uint8_t dataMode; + BitOrder bitOrder; + */ +}; + +extern SPIClass SPI; diff --git a/src/HAL/STM32F1/Servo.cpp b/src/HAL/STM32F1/Servo.cpp new file mode 100644 index 0000000..47ffb63 --- /dev/null +++ b/src/HAL/STM32F1/Servo.cpp @@ -0,0 +1,226 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +uint8_t ServoCount = 0; + +#include "Servo.h" + +//#include "Servo.h" + +#include +#include +#include +#include + +/** + * 20 millisecond period config. For a 1-based prescaler, + * + * (prescaler * overflow / CYC_MSEC) msec = 1 timer cycle = 20 msec + * => prescaler * overflow = 20 * CYC_MSEC + * + * This uses the smallest prescaler that allows an overflow < 2^16. + */ +#define MAX_OVERFLOW UINT16_MAX // _BV(16) - 1 +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) +#define TAU_MSEC 20 +#define TAU_USEC (TAU_MSEC * 1000) +#define TAU_CYC (TAU_MSEC * CYC_MSEC) +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) +#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER)) + +// Unit conversions +#define US_TO_COMPARE(us) uint16_t(map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) +#define COMPARE_TO_US(c) uint32_t(map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) +#define ANGLE_TO_US(a) uint16_t(map((a), minAngle, maxAngle, SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW)) +#define US_TO_ANGLE(us) int16_t(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, minAngle, maxAngle)) + +void libServo::servoWrite(uint8_t inPin, uint16_t duty_cycle) { + #ifdef MF_TIMER_SERVO0 + if (servoIndex == 0) { + pwmSetDuty(duty_cycle); + return; + } + #endif + + timer_dev *tdev = PIN_MAP[inPin].timer_device; + uint8_t tchan = PIN_MAP[inPin].timer_channel; + if (tdev) timer_set_compare(tdev, tchan, duty_cycle); +} + +libServo::libServo() { + servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO; + HAL_timer_set_interrupt_priority(MF_TIMER_SERVO0, SERVO0_TIMER_IRQ_PRIO); +} + +bool libServo::attach(const int32_t inPin, const int32_t inMinAngle, const int32_t inMaxAngle) { + if (servoIndex >= MAX_SERVOS) return false; + if (inPin >= BOARD_NR_GPIO_PINS) return false; + + minAngle = inMinAngle; + maxAngle = inMaxAngle; + angle = -1; + + #ifdef MF_TIMER_SERVO0 + if (servoIndex == 0 && setupSoftPWM(inPin)) { + pin = inPin; // set attached() + return true; + } + #endif + + if (!PWM_PIN(inPin)) return false; + + timer_dev *tdev = PIN_MAP[inPin].timer_device; + //uint8_t tchan = PIN_MAP[inPin].timer_channel; + + SET_PWM(inPin); + servoWrite(inPin, 0); + + timer_pause(tdev); + timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based + timer_set_reload(tdev, SERVO_OVERFLOW); + timer_generate_update(tdev); + timer_resume(tdev); + + pin = inPin; // set attached() + return true; +} + +bool libServo::detach() { + if (!attached()) return false; + angle = -1; + servoWrite(pin, 0); + return true; +} + +int32_t libServo::read() const { + if (attached()) { + #ifdef MF_TIMER_SERVO0 + if (servoIndex == 0) return angle; + #endif + timer_dev *tdev = PIN_MAP[pin].timer_device; + uint8_t tchan = PIN_MAP[pin].timer_channel; + return US_TO_ANGLE(COMPARE_TO_US(timer_get_compare(tdev, tchan))); + } + return 0; +} + +void libServo::move(const int32_t value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + + if (attached()) { + angle = constrain(value, minAngle, maxAngle); + servoWrite(pin, US_TO_COMPARE(ANGLE_TO_US(angle))); + safe_delay(servo_delay[servoIndex]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +#ifdef MF_TIMER_SERVO0 + extern "C" void Servo_IRQHandler() { + static timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); + uint16_t SR = timer_get_status(tdev); + if (SR & TIMER_SR_CC1IF) { // channel 1 off + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(SERVO0_PIN, HIGH); // off + #else + OUT_WRITE(SERVO0_PIN, LOW); + #endif + timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); + } + if (SR & TIMER_SR_CC2IF) { // channel 2 resume + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(SERVO0_PIN, LOW); // on + #else + OUT_WRITE(SERVO0_PIN, HIGH); + #endif + timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); + } + } + + bool libServo::setupSoftPWM(const int32_t inPin) { + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); + if (!tdev) return false; + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(inPin, HIGH); + #else + OUT_WRITE(inPin, LOW); + #endif + + timer_pause(tdev); + timer_set_mode(tdev, 1, TIMER_OUTPUT_COMPARE); // counter with isr + timer_oc_set_mode(tdev, 1, TIMER_OC_MODE_FROZEN, 0); // no pin output change + timer_oc_set_mode(tdev, 2, TIMER_OC_MODE_FROZEN, 0); // no pin output change + timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based + timer_set_reload(tdev, SERVO_OVERFLOW); + timer_set_compare(tdev, 1, SERVO_OVERFLOW); + timer_set_compare(tdev, 2, SERVO_OVERFLOW); + timer_attach_interrupt(tdev, 1, Servo_IRQHandler); + timer_attach_interrupt(tdev, 2, Servo_IRQHandler); + timer_generate_update(tdev); + timer_resume(tdev); + + return true; + } + + void libServo::pwmSetDuty(const uint16_t duty_cycle) { + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); + timer_set_compare(tdev, 1, duty_cycle); + timer_generate_update(tdev); + if (duty_cycle) { + timer_enable_irq(tdev, 1); + timer_enable_irq(tdev, 2); + } + else { + timer_disable_irq(tdev, 1); + timer_disable_irq(tdev, 2); + #ifdef SERVO0_PWM_OD + OUT_WRITE_OD(pin, HIGH); // off + #else + OUT_WRITE(pin, LOW); + #endif + } + } + + void libServo::pauseSoftPWM() { // detach + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); + timer_pause(tdev); + pwmSetDuty(0); + } + +#else + + bool libServo::setupSoftPWM(const int32_t inPin) { return false; } + void libServo::pwmSetDuty(const uint16_t duty_cycle) {} + void libServo::pauseSoftPWM() {} + +#endif + +#endif // HAS_SERVOS + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/Servo.h b/src/HAL/STM32F1/Servo.h new file mode 100644 index 0000000..745a1c9 --- /dev/null +++ b/src/HAL/STM32F1/Servo.h @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// Pin number of unattached pins +#define NOT_ATTACHED (-1) +#define INVALID_SERVO 255 + +#ifndef MAX_SERVOS + #define MAX_SERVOS 3 +#endif + +#define SERVO_DEFAULT_MIN_PW 544 +#define SERVO_DEFAULT_MAX_PW 2400 +#define SERVO_DEFAULT_MIN_ANGLE 0 +#define SERVO_DEFAULT_MAX_ANGLE 180 + +class libServo; +typedef libServo hal_servo_t; + +class libServo { + public: + libServo(); + bool attach(const int32_t pin, const int32_t minAngle=SERVO_DEFAULT_MIN_ANGLE, const int32_t maxAngle=SERVO_DEFAULT_MAX_ANGLE); + bool attached() const { return pin != NOT_ATTACHED; } + bool detach(); + void move(const int32_t value); + int32_t read() const; + private: + void servoWrite(uint8_t pin, const uint16_t duty_cycle); + + uint8_t servoIndex; // index into the channel data for this servo + int32_t pin = NOT_ATTACHED; + int32_t minAngle; + int32_t maxAngle; + int32_t angle; + + bool setupSoftPWM(const int32_t pin); + void pauseSoftPWM(); + void pwmSetDuty(const uint16_t duty_cycle); +}; diff --git a/src/HAL/STM32F1/build_flags.py b/src/HAL/STM32F1/build_flags.py new file mode 100644 index 0000000..970ca8b --- /dev/null +++ b/src/HAL/STM32F1/build_flags.py @@ -0,0 +1,56 @@ +from __future__ import print_function +import sys + +#dynamic build flags for generic compile options +if __name__ == "__main__": + args = " ".join([ "-std=gnu++14", + "-Os", + "-mcpu=cortex-m3", + "-mthumb", + + "-fsigned-char", + "-fno-move-loop-invariants", + "-fno-strict-aliasing", + "-fsingle-precision-constant", + + "--specs=nano.specs", + "--specs=nosys.specs", + + "-IMarlin/src/HAL/STM32F1", + + "-MMD", + "-MP", + "-DTARGET_STM32F1" + ]) + + for i in range(1, len(sys.argv)): + args += " " + sys.argv[i] + + print(args) + +# extra script for linker options +else: + import pioutil + if pioutil.is_pio_build(): + from SCons.Script import DefaultEnvironment + env = DefaultEnvironment() + env.Append( + ARFLAGS=["rcs"], + + ASFLAGS=["-x", "assembler-with-cpp"], + + CXXFLAGS=[ + "-fabi-version=0", + "-fno-use-cxa-atexit", + "-fno-threadsafe-statics" + ], + LINKFLAGS=[ + "-Os", + "-mcpu=cortex-m3", + "-ffreestanding", + "-mthumb", + "--specs=nano.specs", + "--specs=nosys.specs", + "-u_printf_float", + ], + ) diff --git a/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp b/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp new file mode 100644 index 0000000..26ea1ea --- /dev/null +++ b/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp @@ -0,0 +1,170 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef __STM32F1__ + +#include "../../../inc/MarlinConfig.h" + +#if BOTH(HAS_MARLINUI_U8GLIB, FORCE_SOFT_SPI) + +#include +#include "../../shared/HAL_SPI.h" + +#ifndef LCD_SPI_SPEED + #define LCD_SPI_SPEED SPI_FULL_SPEED // Fastest + //#define LCD_SPI_SPEED SPI_QUARTER_SPEED // Slower +#endif + +static uint8_t SPI_speed = LCD_SPI_SPEED; + +static inline uint8_t swSpiTransfer_mode_0(uint8_t b, const uint8_t spi_speed, const pin_t miso_pin=-1) { + LOOP_L_N(i, 8) { + if (spi_speed == 0) { + WRITE(DOGLCD_MOSI, !!(b & 0x80)); + WRITE(DOGLCD_SCK, HIGH); + b <<= 1; + if (miso_pin >= 0 && READ(miso_pin)) b |= 1; + WRITE(DOGLCD_SCK, LOW); + } + else { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + LOOP_L_N(j, spi_speed) + WRITE(DOGLCD_MOSI, state); + + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE(DOGLCD_SCK, HIGH); + + b <<= 1; + if (miso_pin >= 0 && READ(miso_pin)) b |= 1; + + LOOP_L_N(j, spi_speed) + WRITE(DOGLCD_SCK, LOW); + } + } + return b; +} + +static inline uint8_t swSpiTransfer_mode_3(uint8_t b, const uint8_t spi_speed, const pin_t miso_pin=-1) { + LOOP_L_N(i, 8) { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + if (spi_speed == 0) { + WRITE(DOGLCD_SCK, LOW); + WRITE(DOGLCD_MOSI, state); + WRITE(DOGLCD_MOSI, state); // need some setup time + WRITE(DOGLCD_SCK, HIGH); + } + else { + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE(DOGLCD_SCK, LOW); + + LOOP_L_N(j, spi_speed) + WRITE(DOGLCD_MOSI, state); + + LOOP_L_N(j, spi_speed) + WRITE(DOGLCD_SCK, HIGH); + } + b <<= 1; + if (miso_pin >= 0 && READ(miso_pin)) b |= 1; + } + return b; +} + +static void u8g_sw_spi_HAL_STM32F1_shift_out(uint8_t val) { + #if ENABLED(FYSETC_MINI_12864) + swSpiTransfer_mode_3(val, SPI_speed); + #else + swSpiTransfer_mode_0(val, SPI_speed); + #endif +} + +static uint8_t swSpiInit(const uint8_t spi_speed) { + #if PIN_EXISTS(LCD_RESET) + SET_OUTPUT(LCD_RESET_PIN); + #endif + SET_OUTPUT(DOGLCD_A0); + OUT_WRITE(DOGLCD_SCK, LOW); + OUT_WRITE(DOGLCD_MOSI, LOW); + OUT_WRITE(DOGLCD_CS, HIGH); + return spi_speed; +} + +uint8_t u8g_com_HAL_STM32F1_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SPI_speed = swSpiInit(LCD_SPI_SPEED); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + #if PIN_EXISTS(LCD_RESET) + WRITE(LCD_RESET_PIN, arg_val); + #endif + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if ENABLED(FYSETC_MINI_12864) // This LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + WRITE(DOGLCD_SCK, HIGH); // Set SCK to mode 3 idle state before CS goes active + WRITE(DOGLCD_CS, LOW); + } + else { + WRITE(DOGLCD_CS, HIGH); + WRITE(DOGLCD_SCK, LOW); // Set SCK to mode 0 idle state after CS goes inactive + } + #else + WRITE(DOGLCD_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_sw_spi_HAL_STM32F1_shift_out(arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_HAL_STM32F1_shift_out(*ptr++); + arg_val--; + } + } break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_HAL_STM32F1_shift_out(u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + WRITE(DOGLCD_A0, arg_val); + break; + } + return 1; +} + +#endif // HAS_MARLINUI_U8GLIB && FORCE_SOFT_SPI +#endif // STM32F1 diff --git a/src/HAL/STM32F1/eeprom_bl24cxx.cpp b/src/HAL/STM32F1/eeprom_bl24cxx.cpp new file mode 100644 index 0000000..4e25bc6 --- /dev/null +++ b/src/HAL/STM32F1/eeprom_bl24cxx.cpp @@ -0,0 +1,82 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __STM32F1__ + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +// +// PersistentStore +// + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for IIC_BL24CXX_EEPROM." +#endif + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t * const p = (uint8_t * const)pos; + uint8_t c = eeprom_read_byte(p); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // IIC_BL24CXX_EEPROM +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/eeprom_flash.cpp b/src/HAL/STM32F1/eeprom_flash.cpp new file mode 100644 index 0000000..e7d9dd2 --- /dev/null +++ b/src/HAL/STM32F1/eeprom_flash.cpp @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * persistent_store_flash.cpp + * HAL for stm32duino and compatible (STM32F1) + * Implementation of EEPROM settings in SDCard + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(FLASH_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" + +#include +#include + +// Store settings in the last two pages +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE ((EEPROM_PAGE_SIZE) * 2) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +static uint8_t ram_eeprom[MARLIN_EEPROM_SIZE] __attribute__((aligned(4))) = {0}; +static bool eeprom_dirty = false; + +bool PersistentStore::access_start() { + const uint32_t *source = reinterpret_cast(EEPROM_PAGE0_BASE); + uint32_t *destination = reinterpret_cast(ram_eeprom); + + static_assert(0 == (MARLIN_EEPROM_SIZE) % 4, "MARLIN_EEPROM_SIZE is corrupted. (Must be a multiple of 4.)"); // Ensure copying as uint32_t is safe + constexpr size_t eeprom_size_u32 = (MARLIN_EEPROM_SIZE) / 4; + + for (size_t i = 0; i < eeprom_size_u32; ++i, ++destination, ++source) + *destination = *source; + + eeprom_dirty = false; + return true; +} + +bool PersistentStore::access_finish() { + + if (eeprom_dirty) { + FLASH_Status status; + + // Instead of erasing all (both) pages, maybe in the loop we check what page we are in, and if the + // data has changed in that page. We then erase the first time we "detect" a change. In theory, if + // nothing changed in a page, we wouldn't need to erase/write it. + // Or, instead of checking at this point, turn eeprom_dirty into an array of bool the size of number + // of pages. Inside write_data, we set the flag to true at that time if something in that + // page changes...either way, something to look at later. + FLASH_Unlock(); + + #define ACCESS_FINISHED(TF) { FLASH_Lock(); eeprom_dirty = false; return TF; } + + status = FLASH_ErasePage(EEPROM_PAGE0_BASE); + if (status != FLASH_COMPLETE) ACCESS_FINISHED(true); + status = FLASH_ErasePage(EEPROM_PAGE1_BASE); + if (status != FLASH_COMPLETE) ACCESS_FINISHED(true); + + const uint16_t *source = reinterpret_cast(ram_eeprom); + for (size_t i = 0; i < MARLIN_EEPROM_SIZE; i += 2, ++source) { + if (FLASH_ProgramHalfWord(EEPROM_PAGE0_BASE + i, *source) != FLASH_COMPLETE) + ACCESS_FINISHED(false); + } + + ACCESS_FINISHED(true); + } + + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + for (size_t i = 0; i < size; ++i) ram_eeprom[pos + i] = value[i]; + eeprom_dirty = true; + crc16(crc, value, size); + pos += size; + return false; // return true for any error +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uint16_t *crc, const bool writing/*=true*/) { + const uint8_t * const buff = writing ? &value[0] : &ram_eeprom[pos]; + if (writing) for (size_t i = 0; i < size; i++) value[i] = ram_eeprom[pos + i]; + crc16(crc, buff, size); + pos += size; + return false; // return true for any error +} + +#endif // FLASH_EEPROM_EMULATION +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/eeprom_if_iic.cpp b/src/HAL/STM32F1/eeprom_if_iic.cpp new file mode 100644 index 0000000..78b7af0 --- /dev/null +++ b/src/HAL/STM32F1/eeprom_if_iic.cpp @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Platform-independent Arduino functions for I2C EEPROM. + * Enable USE_SHARED_EEPROM if not supplied by the framework. + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../../libs/BL24CXX.h" +#include "../shared/eeprom_if.h" + +void eeprom_init() { BL24CXX::init(); } + +// ------------------------ +// Public functions +// ------------------------ + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::writeOneByte(eeprom_address, value); +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::readOneByte(eeprom_address); +} + +#endif // IIC_BL24CXX_EEPROM +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/eeprom_sdcard.cpp b/src/HAL/STM32F1/eeprom_sdcard.cpp new file mode 100644 index 0000000..d608cce --- /dev/null +++ b/src/HAL/STM32F1/eeprom_sdcard.cpp @@ -0,0 +1,93 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + * Implementation of EEPROM settings in SD Card + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SDCARD_EEPROM_EMULATION) + +#include "../shared/eeprom_api.h" +#include "../../sd/cardreader.h" + +#define EEPROM_FILENAME "eeprom.dat" + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE 0x1000 // 4KB +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +#define _ALIGN(x) __attribute__ ((aligned(x))) // SDIO uint32_t* compat. +static char _ALIGN(4) HAL_eeprom_data[MARLIN_EEPROM_SIZE]; + +bool PersistentStore::access_start() { + if (!card.isMounted()) return false; + + SdFile file, root = card.getroot(); + if (!file.open(&root, EEPROM_FILENAME, O_RDONLY)) + return true; // false aborts the save + + int bytes_read = file.read(HAL_eeprom_data, MARLIN_EEPROM_SIZE); + if (bytes_read < 0) return false; + for (; bytes_read < MARLIN_EEPROM_SIZE; bytes_read++) + HAL_eeprom_data[bytes_read] = 0xFF; + file.close(); + return true; +} + +bool PersistentStore::access_finish() { + if (!card.isMounted()) return false; + + SdFile file, root = card.getroot(); + int bytes_written = 0; + if (file.open(&root, EEPROM_FILENAME, O_CREAT | O_WRITE | O_TRUNC)) { + bytes_written = file.write(HAL_eeprom_data, MARLIN_EEPROM_SIZE); + file.close(); + } + return (bytes_written == MARLIN_EEPROM_SIZE); +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + for (size_t i = 0; i < size; i++) + HAL_eeprom_data[pos + i] = value[i]; + crc16(crc, value, size); + pos += size; + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uint16_t *crc, const bool writing/*=true*/) { + for (size_t i = 0; i < size; i++) { + uint8_t c = HAL_eeprom_data[pos + i]; + if (writing) value[i] = c; + crc16(crc, &c, 1); + } + pos += size; + return false; +} + +#endif // SDCARD_EEPROM_EMULATION +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/eeprom_wired.cpp b/src/HAL/STM32F1/eeprom_wired.cpp new file mode 100644 index 0000000..bc48eef --- /dev/null +++ b/src/HAL/STM32F1/eeprom_wired.cpp @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL PersistentStore for STM32F1 + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for I2C / SPI EEPROM." +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::access_start() { + eeprom_init(); + #if ENABLED(SPI_EEPROM) + #if SPI_CHAN_EEPROM1 == 1 + SET_OUTPUT(BOARD_SPI1_SCK_PIN); + SET_OUTPUT(BOARD_SPI1_MOSI_PIN); + SET_INPUT(BOARD_SPI1_MISO_PIN); + SET_OUTPUT(SPI_EEPROM1_CS_PIN); + #endif + spiInit(0); + #endif + return true; +} + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing && value) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/endstop_interrupts.h b/src/HAL/STM32F1/endstop_interrupts.h new file mode 100644 index 0000000..4d7edb9 --- /dev/null +++ b/src/HAL/STM32F1/endstop_interrupts.h @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop interrupts for Libmaple STM32F1 based targets. + * + * On STM32F, all pins support external interrupt capability. + * Any pin can be used for external interrupts, but there are some restrictions. + * At most 16 different external interrupts can be used at one time. + * Further, you can’t just pick any 16 pins to use. This is because every pin on the STM32 + * connects to what is called an EXTI line, and only one pin per EXTI line can be used for external interrupts at a time + * Check the Reference Manual of the MCU to confirm which line is used by each pin + */ + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(P, endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/STM32F1/fast_pwm.cpp b/src/HAL/STM32F1/fast_pwm.cpp new file mode 100644 index 0000000..297804a --- /dev/null +++ b/src/HAL/STM32F1/fast_pwm.cpp @@ -0,0 +1,85 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#include + +#define NR_TIMERS TERN(STM32_XL_DENSITY, 14, 8) // Maple timers, 14 for STM32_XL_DENSITY (F/G chips), 8 for HIGH density (C D E) + +static uint16_t timer_freq[NR_TIMERS]; + +inline uint8_t timer_and_index_for_pin(const pin_t pin, timer_dev **timer_ptr) { + *timer_ptr = PIN_MAP[pin].timer_device; + for (uint8_t i = 0; i < NR_TIMERS; i++) if (*timer_ptr == HAL_get_timer_dev(i)) + return i; + return 0; +} + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + const uint16_t duty = invert ? v_size - v : v; + if (PWM_PIN(pin)) { + timer_dev *timer; UNUSED(timer); + if (timer_freq[timer_and_index_for_pin(pin, &timer)] == 0) + set_pwm_frequency(pin, PWM_FREQUENCY); + const uint8_t channel = PIN_MAP[pin].timer_channel; + timer_set_compare(timer, channel, duty); + timer_set_mode(timer, channel, TIMER_PWM); // PWM Output Mode + } + else { + pinMode(pin, OUTPUT); + digitalWrite(pin, duty < v_size / 2 ? LOW : HIGH); + } +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer + + timer_dev *timer; UNUSED(timer); + timer_freq[timer_and_index_for_pin(pin, &timer)] = f_desired; + + // Protect used timers + if (timer == HAL_get_timer_dev(MF_TIMER_TEMP)) return; + if (timer == HAL_get_timer_dev(MF_TIMER_STEP)) return; + #if MF_TIMER_PULSE != MF_TIMER_STEP + if (timer == HAL_get_timer_dev(MF_TIMER_PULSE)) return; + #endif + + if (!(timer->regs.bas->SR & TIMER_CR1_CEN)) // Ensure the timer is enabled + timer_init(timer); + + const uint8_t channel = PIN_MAP[pin].timer_channel; + timer_set_mode(timer, channel, TIMER_PWM); + // Preload (resolution) cannot be equal to duty of 255 otherwise it may not result in digital off or on. + uint16_t preload = 254; + int32_t prescaler = (HAL_TIMER_RATE) / (preload + 1) / f_desired - 1; + if (prescaler > 65535) { // For low frequencies increase prescaler + prescaler = 65535; + preload = (HAL_TIMER_RATE) / (prescaler + 1) / f_desired - 1; + } + if (prescaler < 0) return; // Too high frequency + timer_set_reload(timer, preload); + timer_set_prescaler(timer, prescaler); +} + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/fastio.h b/src/HAL/STM32F1/fastio.h new file mode 100644 index 0000000..e75254d --- /dev/null +++ b/src/HAL/STM32F1/fastio.h @@ -0,0 +1,186 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O interfaces for STM32F1 + * These use GPIO functions instead of Direct Port Manipulation, as on AVR. + */ + +#include + +#define READ(IO) (PIN_MAP[IO].gpio_device->regs->IDR & _BV32(PIN_MAP[IO].gpio_bit) ? HIGH : LOW) +#define WRITE(IO,V) (PIN_MAP[IO].gpio_device->regs->BSRR = _BV32(PIN_MAP[IO].gpio_bit) << ((V) ? 0 : 16)) +#define TOGGLE(IO) TBI32(PIN_MAP[IO].gpio_device->regs->ODR, PIN_MAP[IO].gpio_bit) + +#define _GET_MODE(IO) gpio_get_mode(PIN_MAP[IO].gpio_device, PIN_MAP[IO].gpio_bit) +#define _SET_MODE(IO,M) gpio_set_mode(PIN_MAP[IO].gpio_device, PIN_MAP[IO].gpio_bit, M) +#define _SET_OUTPUT(IO) _SET_MODE(IO, GPIO_OUTPUT_PP) +#define _SET_OUTPUT_OD(IO) _SET_MODE(IO, GPIO_OUTPUT_OD) + +#define OUT_WRITE(IO,V) do{ _SET_OUTPUT(IO); WRITE(IO,V); }while(0) +#define OUT_WRITE_OD(IO,V) do{ _SET_OUTPUT_OD(IO); WRITE(IO,V); }while(0) + +#define SET_INPUT(IO) _SET_MODE(IO, GPIO_INPUT_FLOATING) +#define SET_INPUT_PULLUP(IO) _SET_MODE(IO, GPIO_INPUT_PU) +#define SET_INPUT_PULLDOWN(IO) _SET_MODE(IO, GPIO_INPUT_PD) +#define SET_OUTPUT(IO) OUT_WRITE(IO, LOW) +#define SET_PWM(IO) pinMode(IO, PWM) // do{ gpio_set_mode(PIN_MAP[pin].gpio_device, PIN_MAP[pin].gpio_bit, GPIO_AF_OUTPUT_PP); timer_set_mode(PIN_MAP[pin].timer_device, PIN_MAP[pin].timer_channel, TIMER_PWM); }while(0) +#define SET_PWM_OD(IO) pinMode(IO, PWM_OPEN_DRAIN) + +#define IS_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD) +#define IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP || _GET_MODE(IO) == GPIO_OUTPUT_OD) + +#define PWM_PIN(IO) !!PIN_MAP[IO].timer_device + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +// +// Pins Definitions +// +#define PA0 0x00 +#define PA1 0x01 +#define PA2 0x02 +#define PA3 0x03 +#define PA4 0x04 +#define PA5 0x05 +#define PA6 0x06 +#define PA7 0x07 +#define PA8 0x08 +#define PA9 0x09 +#define PA10 0x0A +#define PA11 0x0B +#define PA12 0x0C +#define PA13 0x0D +#define PA14 0x0E +#define PA15 0x0F + +#define PB0 0x10 +#define PB1 0x11 +#define PB2 0x12 +#define PB3 0x13 +#define PB4 0x14 +#define PB5 0x15 +#define PB6 0x16 +#define PB7 0x17 // 36 pins (F103T) +#define PB8 0x18 +#define PB9 0x19 +#define PB10 0x1A +#define PB11 0x1B +#define PB12 0x1C +#define PB13 0x1D +#define PB14 0x1E +#define PB15 0x1F + +#if defined(MCU_STM32F103CB) || defined(MCU_STM32F103C8) + #define PC13 0x20 + #define PC14 0x21 + #define PC15 0x22 +#else + #define PC0 0x20 + #define PC1 0x21 + #define PC2 0x22 + #define PC3 0x23 + #define PC4 0x24 + #define PC5 0x25 + #define PC6 0x26 + #define PC7 0x27 + #define PC8 0x28 + #define PC9 0x29 + #define PC10 0x2A + #define PC11 0x2B + #define PC12 0x2C + #define PC13 0x2D + #define PC14 0x2E + #define PC15 0x2F +#endif + +#define PD0 0x30 +#define PD1 0x31 +#define PD2 0x32 // 64 pins (F103R) +#define PD3 0x33 +#define PD4 0x34 +#define PD5 0x35 +#define PD6 0x36 +#define PD7 0x37 +#define PD8 0x38 +#define PD9 0x39 +#define PD10 0x3A +#define PD11 0x3B +#define PD12 0x3C +#define PD13 0x3D +#define PD14 0x3E +#define PD15 0x3F + +#define PE0 0x40 +#define PE1 0x41 +#define PE2 0x42 +#define PE3 0x43 +#define PE4 0x44 +#define PE5 0x45 +#define PE6 0x46 +#define PE7 0x47 +#define PE8 0x48 +#define PE9 0x49 +#define PE10 0x4A +#define PE11 0x4B +#define PE12 0x4C +#define PE13 0x4D +#define PE14 0x4E +#define PE15 0x4F // 100 pins (F103V) + +#define PF0 0x50 +#define PF1 0x51 +#define PF2 0x52 +#define PF3 0x53 +#define PF4 0x54 +#define PF5 0x55 +#define PF6 0x56 +#define PF7 0x57 +#define PF8 0x58 +#define PF9 0x59 +#define PF10 0x5A +#define PF11 0x5B +#define PF12 0x5C +#define PF13 0x5D +#define PF14 0x5E +#define PF15 0x5F + +#define PG0 0x60 +#define PG1 0x61 +#define PG2 0x62 +#define PG3 0x63 +#define PG4 0x64 +#define PG5 0x65 +#define PG6 0x66 +#define PG7 0x67 +#define PG8 0x68 +#define PG9 0x69 +#define PG10 0x6A +#define PG11 0x6B +#define PG12 0x6C +#define PG13 0x6D +#define PG14 0x6E +#define PG15 0x6F // 144 pins (F103Z) diff --git a/src/HAL/STM32F1/inc/Conditionals_LCD.h b/src/HAL/STM32F1/inc/Conditionals_LCD.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/STM32F1/inc/Conditionals_LCD.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/STM32F1/inc/Conditionals_adv.h b/src/HAL/STM32F1/inc/Conditionals_adv.h new file mode 100644 index 0000000..0fe7924 --- /dev/null +++ b/src/HAL/STM32F1/inc/Conditionals_adv.h @@ -0,0 +1,30 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifdef USE_USB_COMPOSITE + //#warning "SD_CHECK_AND_RETRY isn't needed with USE_USB_COMPOSITE." + #undef SD_CHECK_AND_RETRY + #if DISABLED(NO_SD_HOST_DRIVE) + #define HAS_SD_HOST_DRIVE 1 + #endif +#endif diff --git a/src/HAL/STM32F1/inc/Conditionals_post.h b/src/HAL/STM32F1/inc/Conditionals_post.h new file mode 100644 index 0000000..656fbe1 --- /dev/null +++ b/src/HAL/STM32F1/inc/Conditionals_post.h @@ -0,0 +1,34 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// If no real EEPROM, Flash emulation, or SRAM emulation is available fall back to SD emulation +#if USE_FALLBACK_EEPROM + #define SDCARD_EEPROM_EMULATION +#elif EITHER(I2C_EEPROM, SPI_EEPROM) + #define USE_SHARED_EEPROM 1 +#endif + +// Allow SDSUPPORT to be disabled +#if DISABLED(SDSUPPORT) + #undef SDIO_SUPPORT +#endif diff --git a/src/HAL/STM32F1/inc/SanityCheck.h b/src/HAL/STM32F1/inc/SanityCheck.h new file mode 100644 index 0000000..fe8f6e0 --- /dev/null +++ b/src/HAL/STM32F1/inc/SanityCheck.h @@ -0,0 +1,51 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test STM32F1-specific configuration values for errors at compile-time. + */ + +#if ENABLED(SDCARD_EEPROM_EMULATION) && DISABLED(SDSUPPORT) + #undef SDCARD_EEPROM_EMULATION // Avoid additional error noise + #if USE_FALLBACK_EEPROM + #warning "EEPROM type not specified. Fallback is SDCARD_EEPROM_EMULATION." + #endif + #error "SDCARD_EEPROM_EMULATION requires SDSUPPORT. Enable SDSUPPORT or choose another EEPROM emulation." +#endif + +#if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + #error "SERIAL_STATS_MAX_RX_QUEUED is not supported on the STM32F1 platform." +#elif ENABLED(SERIAL_STATS_DROPPED_RX) + #error "SERIAL_STATS_DROPPED_RX is not supported on the STM32F1 platform." +#endif + +#if ENABLED(NEOPIXEL_LED) && DISABLED(FYSETC_MINI_12864_2_1) + #error "NEOPIXEL_LED (Adafruit NeoPixel) is not supported for HAL/STM32F1. Comment out this line to proceed at your own risk!" +#endif + +// Emergency Parser needs at least one serial with HardwareSerial or USBComposite. +// The USBSerial maple don't allow any hook to implement EMERGENCY_PARSER. +// And copy all USBSerial code to marlin space to support EMERGENCY_PARSER, when we have another options, don't worth it. +#if ENABLED(EMERGENCY_PARSER) && !defined(USE_USB_COMPOSITE) && ((SERIAL_PORT == -1 && !defined(SERIAL_PORT_2)) || (SERIAL_PORT_2 == -1 && !defined(SERIAL_PORT))) + #error "EMERGENCY_PARSER is only supported by HardwareSerial or USBComposite in HAL/STM32F1." +#endif diff --git a/src/HAL/STM32F1/maple_win_usb_driver/maple_serial.inf b/src/HAL/STM32F1/maple_win_usb_driver/maple_serial.inf new file mode 100644 index 0000000..e4221b7 --- /dev/null +++ b/src/HAL/STM32F1/maple_win_usb_driver/maple_serial.inf @@ -0,0 +1,56 @@ +; +; STMicroelectronics Communication Device Class driver installation file +; (C)2006 Copyright STMicroelectronics +; + +[Version] +Signature="$Windows NT$" +Class=Ports +ClassGuid={4D36E978-E325-11CE-BFC1-08002BE10318} +Provider=%STM% +LayoutFile=layout.inf + +[Manufacturer] +%MFGNAME%=VirComDevice,NT,NTamd64 + +[DestinationDirs] +DefaultDestDir = 12 + +[VirComDevice.NT] +%DESCRIPTION%=DriverInstall,USB\VID_1EAF&PID_0029&MI_01 +%DESCRIPTION%=DriverInstall,USB\VID_1EAF&PID_0029&MI_01 + +[VirComDevice.NTamd64] +%DESCRIPTION%=DriverInstall,USB\VID_1EAF&PID_0029&MI_01 +%DESCRIPTION%=DriverInstall,USB\VID_1EAF&PID_0029&MI_01 + +[DriverInstall.NT] +Include=mdmcpq.inf +CopyFiles=FakeModemCopyFileSection +AddReg=DriverInstall.NT.AddReg + +[DriverInstall.NT.AddReg] +HKR,,DevLoader,,*ntkern +HKR,,NTMPDriver,,usbser.sys +HKR,,EnumPropPages32,,"MsPorts.dll,SerialPortPropPageProvider" + +[DriverInstall.NT.Services] +AddService=usbser, 0x00000002, DriverServiceInst + +[DriverServiceInst] +DisplayName=%SERVICE% +ServiceType=1 +StartType=3 +ErrorControl=1 +ServiceBinary=%12%\usbser.sys + +;------------------------------------------------------------------------------ +; String Definitions +;------------------------------------------------------------------------------ + + +[Strings] +STM = "LeafLabs" +MFGNAME = "LeafLabs" +DESCRIPTION = "Maple R3" +SERVICE = "USB Virtual COM port" diff --git a/src/HAL/STM32F1/msc_sd.cpp b/src/HAL/STM32F1/msc_sd.cpp new file mode 100644 index 0000000..f490c83 --- /dev/null +++ b/src/HAL/STM32F1/msc_sd.cpp @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] + * + * This program 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_SD_HOST_DRIVE + +#include "msc_sd.h" +#include "SPI.h" +#include "usb_reg_map.h" + +#define PRODUCT_ID 0x29 + +USBMassStorage MarlinMSC; +Serial1Class MarlinCompositeSerial(true); + +#include "../../inc/MarlinConfig.h" + +#if SD_CONNECTION_IS(ONBOARD) + + #include "onboard_sd.h" + + static bool MSC_Write(const uint8_t *writebuff, uint32_t startSector, uint16_t numSectors) { + return (disk_write(0, writebuff, startSector, numSectors) == RES_OK); + } + static bool MSC_Read(uint8_t *readbuff, uint32_t startSector, uint16_t numSectors) { + return (disk_read(0, readbuff, startSector, numSectors) == RES_OK); + } + +#endif + +#if ENABLED(EMERGENCY_PARSER) + + // The original callback is not called (no way to retrieve address). + // That callback detects a special STM32 reset sequence: this functionality is not essential + // as M997 achieves the same. + void my_rx_callback(unsigned int, void*) { + // max length of 16 is enough to contain all emergency commands + uint8 buf[16]; + + //rx is usbSerialPart.endpoints[2] + uint16 len = usb_get_ep_rx_count(usbSerialPart.endpoints[2].address); + uint32 total = composite_cdcacm_data_available(); + + if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf))) + return; + + // cannot get character by character due to bug in composite_cdcacm_peek_ex + len = composite_cdcacm_peek(buf, total); + + for (uint32 i = 0; i < len; i++) + emergency_parser.update(MarlinCompositeSerial.emergency_state, buf[i+total-len]); + } + +#endif + +void MSC_SD_init() { + USBComposite.setProductId(PRODUCT_ID); + // Just set MarlinCompositeSerial enabled to true + // because when MarlinCompositeSerial.begin() is used in setup() + // it clears all USBComposite devices. + MarlinCompositeSerial.begin(); + USBComposite.end(); + USBComposite.clear(); + // Set api and register mass storage + #if SD_CONNECTION_IS(ONBOARD) + uint32_t cardSize; + if (disk_initialize(0) == RES_OK) { + if (disk_ioctl(0, GET_SECTOR_COUNT, (void *)(&cardSize)) == RES_OK) { + MarlinMSC.setDriveData(0, cardSize, MSC_Read, MSC_Write); + MarlinMSC.registerComponent(); + } + } + #endif + // Register composite Serial + MarlinCompositeSerial.registerComponent(); + USBComposite.begin(); + #if ENABLED(EMERGENCY_PARSER) + composite_cdcacm_set_hooks(USBHID_CDCACM_HOOK_RX, my_rx_callback); + #endif +} + +#endif // HAS_SD_HOST_DRIVE +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/msc_sd.h b/src/HAL/STM32F1/msc_sd.h new file mode 100644 index 0000000..f4636bd --- /dev/null +++ b/src/HAL/STM32F1/msc_sd.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] + * + * This program 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +#include "../../inc/MarlinConfigPre.h" +#include "../../core/serial_hook.h" + +extern USBMassStorage MarlinMSC; +extern Serial1Class MarlinCompositeSerial; + +void MSC_SD_init(); diff --git a/src/HAL/STM32F1/onboard_sd.cpp b/src/HAL/STM32F1/onboard_sd.cpp new file mode 100644 index 0000000..a3d8dcb --- /dev/null +++ b/src/HAL/STM32F1/onboard_sd.cpp @@ -0,0 +1,571 @@ +/** + * STM32F1: MMCv3/SDv1/SDv2 (SPI mode) control module + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] + * Copyright (C) 2015, ChaN, all right reserved. + * + * This software is a free software and there is NO WARRANTY. + * No restriction on use. You can use, modify and redistribute it for + * personal, non-profit or commercial products UNDER YOUR RESPONSIBILITY. + * Redistributions of source code must retain the above copyright notice. + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +#if SD_CONNECTION_IS(ONBOARD) + +#include "onboard_sd.h" +#include "SPI.h" +#include "fastio.h" + +#ifndef ONBOARD_SPI_DEVICE + #define ONBOARD_SPI_DEVICE SPI_DEVICE +#endif + +#if HAS_SD_HOST_DRIVE + #define ONBOARD_SD_SPI SPI +#else + SPIClass OnboardSPI(ONBOARD_SPI_DEVICE); + #define ONBOARD_SD_SPI OnboardSPI +#endif + +#if ONBOARD_SPI_DEVICE == 1 + #define SPI_CLOCK_MAX SPI_BAUD_PCLK_DIV_4 +#else + #define SPI_CLOCK_MAX SPI_BAUD_PCLK_DIV_2 +#endif + +#if PIN_EXISTS(ONBOARD_SD_CS) && ONBOARD_SD_CS_PIN != SD_SS_PIN + #define CS_LOW() WRITE(ONBOARD_SD_CS_PIN, LOW) // Set OnboardSPI cs low + #define CS_HIGH() WRITE(ONBOARD_SD_CS_PIN, HIGH) // Set OnboardSPI cs high +#else + #define CS_LOW() + #define CS_HIGH() +#endif + +#define FCLK_FAST() ONBOARD_SD_SPI.setClockDivider(SPI_CLOCK_MAX) +#define FCLK_SLOW() ONBOARD_SD_SPI.setClockDivider(SPI_BAUD_PCLK_DIV_256) + +/*-------------------------------------------------------------------------- + Module Private Functions +---------------------------------------------------------------------------*/ + +/* MMC/SD command */ +#define CMD0 (0) // GO_IDLE_STATE +#define CMD1 (1) // SEND_OP_COND (MMC) +#define ACMD41 (0x80+41) // SEND_OP_COND (SDC) +#define CMD8 (8) // SEND_IF_COND +#define CMD9 (9) // SEND_CSD +#define CMD10 (10) // SEND_CID +#define CMD12 (12) // STOP_TRANSMISSION +#define ACMD13 (0x80+13) // SD_STATUS (SDC) +#define CMD16 (16) // SET_BLOCKLEN +#define CMD17 (17) // READ_SINGLE_BLOCK +#define CMD18 (18) // READ_MULTIPLE_BLOCK +#define CMD23 (23) // SET_BLOCK_COUNT (MMC) +#define ACMD23 (0x80+23) // SET_WR_BLK_ERASE_COUNT (SDC) +#define CMD24 (24) // WRITE_BLOCK +#define CMD25 (25) // WRITE_MULTIPLE_BLOCK +#define CMD32 (32) // ERASE_ER_BLK_START +#define CMD33 (33) // ERASE_ER_BLK_END +#define CMD38 (38) // ERASE +#define CMD48 (48) // READ_EXTR_SINGLE +#define CMD49 (49) // WRITE_EXTR_SINGLE +#define CMD55 (55) // APP_CMD +#define CMD58 (58) // READ_OCR + +static volatile DSTATUS Stat = STA_NOINIT; // Physical drive status +static volatile UINT timeout; +static BYTE CardType; // Card type flags + +/*-----------------------------------------------------------------------*/ +/* Send/Receive data to the MMC (Platform dependent) */ +/*-----------------------------------------------------------------------*/ + +/* Exchange a byte */ +static BYTE xchg_spi ( + BYTE dat // Data to send +) { + BYTE returnByte = ONBOARD_SD_SPI.transfer(dat); + return returnByte; +} + +/* Receive multiple byte */ +static void rcvr_spi_multi ( + BYTE *buff, // Pointer to data buffer + UINT btr // Number of bytes to receive (16, 64 or 512) +) { + ONBOARD_SD_SPI.dmaTransfer(0, const_cast(buff), btr); +} + +#if _DISKIO_WRITE + + // Send multiple bytes + static void xmit_spi_multi ( + const BYTE *buff, // Pointer to the data + UINT btx // Number of bytes to send (multiple of 16) + ) { + ONBOARD_SD_SPI.dmaSend(const_cast(buff), btx); + } + +#endif // _DISKIO_WRITE + +/*-----------------------------------------------------------------------*/ +/* Wait for card ready */ +/*-----------------------------------------------------------------------*/ + +static int wait_ready ( // 1:Ready, 0:Timeout + UINT wt // Timeout [ms] +) { + BYTE d; + timeout = millis() + wt; + do { + d = xchg_spi(0xFF); + // This loop takes a while. Insert rot_rdq() here for multitask environment. + } while (d != 0xFF && (timeout > millis())); // Wait for card goes ready or timeout + + return (d == 0xFF) ? 1 : 0; +} + +/*-----------------------------------------------------------------------*/ +/* Deselect card and release SPI */ +/*-----------------------------------------------------------------------*/ + +static void deselect() { + CS_HIGH(); // CS = H + xchg_spi(0xFF); // Dummy clock (force DO hi-z for multiple slave SPI) +} + +/*-----------------------------------------------------------------------*/ +/* Select card and wait for ready */ +/*-----------------------------------------------------------------------*/ + +static int select() { // 1:OK, 0:Timeout + CS_LOW(); // CS = L + xchg_spi(0xFF); // Dummy clock (force DO enabled) + + if (wait_ready(500)) return 1; // Leading busy check: Wait for card ready + + deselect(); // Timeout + return 0; +} + +/*-----------------------------------------------------------------------*/ +/* Control SPI module (Platform dependent) */ +/*-----------------------------------------------------------------------*/ + +// Enable SSP module and attach it to I/O pads +static void sd_power_on() { + ONBOARD_SD_SPI.setModule(ONBOARD_SPI_DEVICE); + ONBOARD_SD_SPI.begin(); + ONBOARD_SD_SPI.setBitOrder(MSBFIRST); + ONBOARD_SD_SPI.setDataMode(SPI_MODE0); + CS_HIGH(); +} + +// Disable SPI function +static void sd_power_off() { + select(); // Wait for card ready + deselect(); +} + +/*-----------------------------------------------------------------------*/ +/* Receive a data packet from the MMC */ +/*-----------------------------------------------------------------------*/ + +static int rcvr_datablock ( // 1:OK, 0:Error + BYTE *buff, // Data buffer + UINT btr // Data block length (byte) +) { + BYTE token; + + timeout = millis() + 200; + do { // Wait for DataStart token in timeout of 200ms + token = xchg_spi(0xFF); + // This loop will take a while. Insert rot_rdq() here for multitask environment. + } while ((token == 0xFF) && (timeout > millis())); + if (token != 0xFE) return 0; // Function fails if invalid DataStart token or timeout + + rcvr_spi_multi(buff, btr); // Store trailing data to the buffer + xchg_spi(0xFF); xchg_spi(0xFF); // Discard CRC + + return 1; // Function succeeded +} + +/*-----------------------------------------------------------------------*/ +/* Send a data packet to the MMC */ +/*-----------------------------------------------------------------------*/ + +#if _DISKIO_WRITE + + static int xmit_datablock( // 1:OK, 0:Failed + const BYTE *buff, // Pointer to 512 byte data to be sent + BYTE token // Token + ) { + BYTE resp; + + if (!wait_ready(500)) return 0; // Leading busy check: Wait for card ready to accept data block + + xchg_spi(token); // Send token + if (token == 0xFD) return 1; // Do not send data if token is StopTran + + xmit_spi_multi(buff, 512); // Data + xchg_spi(0xFF); xchg_spi(0xFF); // Dummy CRC + + resp = xchg_spi(0xFF); // Receive data resp + + return (resp & 0x1F) == 0x05 ? 1 : 0; // Data was accepted or not + + // Busy check is done at next transmission + } + +#endif // _DISKIO_WRITE + +/*-----------------------------------------------------------------------*/ +/* Send a command packet to the MMC */ +/*-----------------------------------------------------------------------*/ + +static BYTE send_cmd( // Return value: R1 resp (bit7==1:Failed to send) + BYTE cmd, // Command index + DWORD arg // Argument +) { + BYTE n, res; + + if (cmd & 0x80) { // Send a CMD55 prior to ACMD + cmd &= 0x7F; + res = send_cmd(CMD55, 0); + if (res > 1) return res; + } + + // Select the card and wait for ready except to stop multiple block read + if (cmd != CMD12) { + deselect(); + if (!select()) return 0xFF; + } + + // Send command packet + xchg_spi(0x40 | cmd); // Start + command index + xchg_spi((BYTE)(arg >> 24)); // Argument[31..24] + xchg_spi((BYTE)(arg >> 16)); // Argument[23..16] + xchg_spi((BYTE)(arg >> 8)); // Argument[15..8] + xchg_spi((BYTE)arg); // Argument[7..0] + n = 0x01; // Dummy CRC + Stop + if (cmd == CMD0) n = 0x95; // Valid CRC for CMD0(0) + if (cmd == CMD8) n = 0x87; // Valid CRC for CMD8(0x1AA) + xchg_spi(n); + + // Receive command response + if (cmd == CMD12) xchg_spi(0xFF); // Discard the following byte when CMD12 + n = 10; // Wait for response (10 bytes max) + do + res = xchg_spi(0xFF); + while ((res & 0x80) && --n); + + return res; // Return received response +} + +/*-------------------------------------------------------------------------- + Public Functions +---------------------------------------------------------------------------*/ + +/*-----------------------------------------------------------------------*/ +/* Initialize disk drive */ +/*-----------------------------------------------------------------------*/ + +DSTATUS disk_initialize ( + BYTE drv // Physical drive number (0) +) { + BYTE n, cmd, ty, ocr[4]; + + if (drv) return STA_NOINIT; // Supports only drive 0 + sd_power_on(); // Initialize SPI + + if (Stat & STA_NODISK) return Stat; // Is a card existing in the socket? + + FCLK_SLOW(); + for (n = 10; n; n--) xchg_spi(0xFF); // Send 80 dummy clocks + + ty = 0; + if (send_cmd(CMD0, 0) == 1) { // Put the card SPI state + timeout = millis() + 1000; // Initialization timeout = 1 sec + if (send_cmd(CMD8, 0x1AA) == 1) { // Is the catd SDv2? + for (n = 0; n < 4; n++) ocr[n] = xchg_spi(0xFF); // Get 32 bit return value of R7 resp + if (ocr[2] == 0x01 && ocr[3] == 0xAA) { // Does the card support 2.7-3.6V? + while ((timeout > millis()) && send_cmd(ACMD41, 1UL << 30)); // Wait for end of initialization with ACMD41(HCS) + if ((timeout > millis()) && send_cmd(CMD58, 0) == 0) { // Check CCS bit in the OCR + for (n = 0; n < 4; n++) ocr[n] = xchg_spi(0xFF); + ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; // Check if the card is SDv2 + } + } + } + else { // Not an SDv2 card + if (send_cmd(ACMD41, 0) <= 1) { // SDv1 or MMCv3? + ty = CT_SD1; cmd = ACMD41; // SDv1 (ACMD41(0)) + } + else { + ty = CT_MMC; cmd = CMD1; // MMCv3 (CMD1(0)) + } + while ((timeout > millis()) && send_cmd(cmd, 0)); // Wait for the card leaves idle state + if (!(timeout > millis()) || send_cmd(CMD16, 512) != 0) // Set block length: 512 + ty = 0; + } + } + CardType = ty; // Card type + deselect(); + + if (ty) { // OK + FCLK_FAST(); // Set fast clock + Stat &= ~STA_NOINIT; // Clear STA_NOINIT flag + } + else { // Failed + sd_power_off(); + Stat = STA_NOINIT; + } + + return Stat; +} + +/*-----------------------------------------------------------------------*/ +/* Get disk status */ +/*-----------------------------------------------------------------------*/ + +DSTATUS disk_status ( + BYTE drv // Physical drive number (0) +) { + if (drv) return STA_NOINIT; // Supports only drive 0 + return Stat; // Return disk status +} + +/*-----------------------------------------------------------------------*/ +/* Read sector(s) */ +/*-----------------------------------------------------------------------*/ + +DRESULT disk_read ( + BYTE drv, // Physical drive number (0) + BYTE *buff, // Pointer to the data buffer to store read data + DWORD sector, // Start sector number (LBA) + UINT count // Number of sectors to read (1..128) +) { + BYTE cmd; + + if (drv || !count) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check if drive is ready + if (!(CardType & CT_BLOCK)) sector *= 512; // LBA ot BA conversion (byte addressing cards) + FCLK_FAST(); + cmd = count > 1 ? CMD18 : CMD17; // READ_MULTIPLE_BLOCK : READ_SINGLE_BLOCK + if (send_cmd(cmd, sector) == 0) { + do { + if (!rcvr_datablock(buff, 512)) break; + buff += 512; + } while (--count); + if (cmd == CMD18) send_cmd(CMD12, 0); // STOP_TRANSMISSION + } + deselect(); + + return count ? RES_ERROR : RES_OK; // Return result +} + +/*-----------------------------------------------------------------------*/ +/* Write sector(s) */ +/*-----------------------------------------------------------------------*/ + +#if _DISKIO_WRITE + + DRESULT disk_write( + BYTE drv, // Physical drive number (0) + const BYTE *buff, // Pointer to the data to write + DWORD sector, // Start sector number (LBA) + UINT count // Number of sectors to write (1..128) + ) { + if (drv || !count) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check drive status + if (Stat & STA_PROTECT) return RES_WRPRT; // Check write protect + FCLK_FAST(); + if (!(CardType & CT_BLOCK)) sector *= 512; // LBA ==> BA conversion (byte addressing cards) + + if (count == 1) { // Single sector write + if ((send_cmd(CMD24, sector) == 0) // WRITE_BLOCK + && xmit_datablock(buff, 0xFE)) { + count = 0; + } + } + else { // Multiple sector write + if (CardType & CT_SDC) send_cmd(ACMD23, count); // Predefine number of sectors + if (send_cmd(CMD25, sector) == 0) { // WRITE_MULTIPLE_BLOCK + do { + if (!xmit_datablock(buff, 0xFC)) break; + buff += 512; + } while (--count); + if (!xmit_datablock(0, 0xFD)) count = 1; // STOP_TRAN token + } + } + deselect(); + + return count ? RES_ERROR : RES_OK; // Return result + } + +#endif // _DISKIO_WRITE + +/*-----------------------------------------------------------------------*/ +/* Miscellaneous drive controls other than data read/write */ +/*-----------------------------------------------------------------------*/ + +#if _DISKIO_IOCTL + + DRESULT disk_ioctl ( + BYTE drv, // Physical drive number (0) + BYTE cmd, // Control command code + void *buff // Pointer to the conrtol data + ) { + DRESULT res; + BYTE n, csd[16], *ptr = (BYTE *)buff; + DWORD *dp, st, ed, csize; + #if _DISKIO_ISDIO + SDIO_CMD *sdio = buff; + BYTE rc, *buf; + UINT dc; + #endif + + if (drv) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check if drive is ready + + res = RES_ERROR; + FCLK_FAST(); + switch (cmd) { + case CTRL_SYNC: // Wait for end of internal write process of the drive + if (select()) res = RES_OK; + break; + + case GET_SECTOR_COUNT: // Get drive capacity in unit of sector (DWORD) + if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) { + if ((csd[0] >> 6) == 1) { // SDC ver 2.00 + csize = csd[9] + ((WORD)csd[8] << 8) + ((DWORD)(csd[7] & 63) << 16) + 1; + *(DWORD*)buff = csize << 10; + } + else { // SDC ver 1.XX or MMC ver 3 + n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2; + csize = (csd[8] >> 6) + ((WORD)csd[7] << 2) + ((WORD)(csd[6] & 3) << 10) + 1; + *(DWORD*)buff = csize << (n - 9); + } + res = RES_OK; + } + break; + + case GET_BLOCK_SIZE: // Get erase block size in unit of sector (DWORD) + if (CardType & CT_SD2) { // SDC ver 2.00 + if (send_cmd(ACMD13, 0) == 0) { // Read SD status + xchg_spi(0xFF); + if (rcvr_datablock(csd, 16)) { // Read partial block + for (n = 64 - 16; n; n--) xchg_spi(0xFF); // Purge trailing data + *(DWORD*)buff = 16UL << (csd[10] >> 4); + res = RES_OK; + } + } + } + else { // SDC ver 1.XX or MMC + if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) { // Read CSD + if (CardType & CT_SD1) { // SDC ver 1.XX + *(DWORD*)buff = (((csd[10] & 63) << 1) + ((WORD)(csd[11] & 128) >> 7) + 1) << ((csd[13] >> 6) - 1); + } + else { // MMC + *(DWORD*)buff = ((WORD)((csd[10] & 124) >> 2) + 1) * (((csd[11] & 3) << 3) + ((csd[11] & 224) >> 5) + 1); + } + res = RES_OK; + } + } + break; + + case CTRL_TRIM: // Erase a block of sectors (used when _USE_TRIM in ffconf.h is 1) + if (!(CardType & CT_SDC)) break; // Check if the card is SDC + if (disk_ioctl(drv, MMC_GET_CSD, csd)) break; // Get CSD + if (!(csd[0] >> 6) && !(csd[10] & 0x40)) break; // Check if sector erase can be applied to the card + dp = (DWORD *)buff; st = dp[0]; ed = dp[1]; // Load sector block + if (!(CardType & CT_BLOCK)) { + st *= 512; ed *= 512; + } + if (send_cmd(CMD32, st) == 0 && send_cmd(CMD33, ed) == 0 && send_cmd(CMD38, 0) == 0 && wait_ready(30000)) { // Erase sector block + res = RES_OK; // FatFs does not check result of this command + } + break; + + // The following commands are never used by FatFs module + + case MMC_GET_TYPE: // Get MMC/SDC type (BYTE) + *ptr = CardType; + res = RES_OK; + break; + + case MMC_GET_CSD: // Read CSD (16 bytes) + if (send_cmd(CMD9, 0) == 0 && rcvr_datablock(ptr, 16)) { + res = RES_OK; + } + break; + + case MMC_GET_CID: // Read CID (16 bytes) + if (send_cmd(CMD10, 0) == 0 && rcvr_datablock(ptr, 16)) { + res = RES_OK; + } + break; + + case MMC_GET_OCR: // Read OCR (4 bytes) + if (send_cmd(CMD58, 0) == 0) { + for (n = 4; n; n--) *ptr++ = xchg_spi(0xFF); + res = RES_OK; + } + break; + + case MMC_GET_SDSTAT: // Read SD status (64 bytes) + if (send_cmd(ACMD13, 0) == 0) { + xchg_spi(0xFF); + if (rcvr_datablock(ptr, 64)) res = RES_OK; + } + break; + + #if _DISKIO_ISDIO + + case ISDIO_READ: + sdio = buff; + if (send_cmd(CMD48, 0x80000000 | sdio->func << 28 | sdio->addr << 9 | ((sdio->ndata - 1) & 0x1FF)) == 0) { + for (Timer1 = 1000; (rc = xchg_spi(0xFF)) == 0xFF && Timer1; ) ; + if (rc == 0xFE) { + for (buf = sdio->data, dc = sdio->ndata; dc; dc--) *buf++ = xchg_spi(0xFF); + for (dc = 514 - sdio->ndata; dc; dc--) xchg_spi(0xFF); + res = RES_OK; + } + } + break; + case ISDIO_WRITE: + sdio = buff; + if (send_cmd(CMD49, 0x80000000 | sdio->func << 28 | sdio->addr << 9 | ((sdio->ndata - 1) & 0x1FF)) == 0) { + xchg_spi(0xFF); xchg_spi(0xFE); + for (buf = sdio->data, dc = sdio->ndata; dc; dc--) xchg_spi(*buf++); + for (dc = 514 - sdio->ndata; dc; dc--) xchg_spi(0xFF); + if ((xchg_spi(0xFF) & 0x1F) == 0x05) res = RES_OK; + } + break; + case ISDIO_MRITE: + sdio = buff; + if (send_cmd(CMD49, 0x84000000 | sdio->func << 28 | sdio->addr << 9 | sdio->ndata >> 8) == 0) { + xchg_spi(0xFF); xchg_spi(0xFE); + xchg_spi(sdio->ndata); + for (dc = 513; dc; dc--) xchg_spi(0xFF); + if ((xchg_spi(0xFF) & 0x1F) == 0x05) res = RES_OK; + } + break; + + #endif // _DISKIO_ISDIO + + default: res = RES_PARERR; + } + + deselect(); + return res; + } + +#endif // _DISKIO_IOCTL + +#endif // SD_CONNECTION_IS(ONBOARD) +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/onboard_sd.h b/src/HAL/STM32F1/onboard_sd.h new file mode 100644 index 0000000..f228d06 --- /dev/null +++ b/src/HAL/STM32F1/onboard_sd.h @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------- +/ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] +/ * Copyright (c) 2019 BigTreeTech [https://github.com/bigtreetech] +/ * Low level disk interface module include file (C)ChaN, 2015 +/-----------------------------------------------------------------------*/ + +#pragma once + +#define _DISKIO_WRITE 1 /* 1: Enable disk_write function */ +#define _DISKIO_IOCTL 1 /* 1: Enable disk_ioctl function */ +#define _DISKIO_ISDIO 0 /* 1: Enable iSDIO control function */ + +typedef unsigned char BYTE; +typedef unsigned short WORD; +typedef unsigned long DWORD; +typedef unsigned int UINT; + +/* Status of Disk Functions */ +typedef BYTE DSTATUS; + +/* Results of Disk Functions */ +typedef enum { + RES_OK = 0, /* 0: Successful */ + RES_ERROR, /* 1: R/W Error */ + RES_WRPRT, /* 2: Write Protected */ + RES_NOTRDY, /* 3: Not Ready */ + RES_PARERR /* 4: Invalid Parameter */ +} DRESULT; + + +#if _DISKIO_ISDIO +/* Command structure for iSDIO ioctl command */ +typedef struct { + BYTE func; /* Function number: 0..7 */ + WORD ndata; /* Number of bytes to transfer: 1..512, or mask + data */ + DWORD addr; /* Register address: 0..0x1FFFF */ + void* data; /* Pointer to the data (to be written | read buffer) */ +} SDIO_CMD; +#endif + +/*---------------------------------------*/ +/* Prototypes for disk control functions */ + +DSTATUS disk_initialize(BYTE pdrv); +DSTATUS disk_status(BYTE pdrv); +DRESULT disk_read(BYTE pdrv, BYTE* buff, DWORD sector, UINT count); +#if _DISKIO_WRITE + DRESULT disk_write(BYTE pdrv, const BYTE* buff, DWORD sector, UINT count); +#endif +#if _DISKIO_IOCTL + DRESULT disk_ioctl(BYTE pdrv, BYTE cmd, void *buff); +#endif + +/* Disk Status Bits (DSTATUS) */ +#define STA_NOINIT 0x01 /* Drive not initialized */ +#define STA_NODISK 0x02 /* No medium in the drive */ +#define STA_PROTECT 0x04 /* Write protected */ + +/* Command code for disk_ioctrl function */ + +/* Generic command (Used by FatFs) */ +#define CTRL_SYNC 0 /* Complete pending write process (needed at _FS_READONLY == 0) */ +#define GET_SECTOR_COUNT 1 /* Get media size (needed at _USE_MKFS == 1) */ +#define GET_SECTOR_SIZE 2 /* Get sector size (needed at _MAX_SS != _MIN_SS) */ +#define GET_BLOCK_SIZE 3 /* Get erase block size (needed at _USE_MKFS == 1) */ +#define CTRL_TRIM 4 /* Inform device that the data on the block of sectors is no longer used (needed at _USE_TRIM == 1) */ + +/* Generic command (Not used by FatFs) */ +#define CTRL_FORMAT 5 /* Create physical format on the media */ +#define CTRL_POWER_IDLE 6 /* Put the device idle state */ +#define CTRL_POWER_OFF 7 /* Put the device off state */ +#define CTRL_LOCK 8 /* Lock media removal */ +#define CTRL_UNLOCK 9 /* Unlock media removal */ +#define CTRL_EJECT 10 /* Eject media */ + +/* MMC/SDC specific ioctl command (Not used by FatFs) */ +#define MMC_GET_TYPE 50 /* Get card type */ +#define MMC_GET_CSD 51 /* Get CSD */ +#define MMC_GET_CID 52 /* Get CID */ +#define MMC_GET_OCR 53 /* Get OCR */ +#define MMC_GET_SDSTAT 54 /* Get SD status */ +#define ISDIO_READ 55 /* Read data form SD iSDIO register */ +#define ISDIO_WRITE 56 /* Write data to SD iSDIO register */ +#define ISDIO_MRITE 57 /* Masked write data to SD iSDIO register */ + +/* ATA/CF specific ioctl command (Not used by FatFs) */ +#define ATA_GET_REV 60 /* Get F/W revision */ +#define ATA_GET_MODEL 61 /* Get model name */ +#define ATA_GET_SN 62 /* Get serial number */ + +/* MMC card type flags (MMC_GET_TYPE) */ +#define CT_MMC 0x01 /* MMC ver 3 */ +#define CT_SD1 0x02 /* SD ver 1 */ +#define CT_SD2 0x04 /* SD ver 2 */ +#define CT_SDC (CT_SD1|CT_SD2) /* SD */ +#define CT_BLOCK 0x08 /* Block addressing */ diff --git a/src/HAL/STM32F1/pinsDebug.h b/src/HAL/STM32F1/pinsDebug.h new file mode 100644 index 0000000..7828479 --- /dev/null +++ b/src/HAL/STM32F1/pinsDebug.h @@ -0,0 +1,123 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Support routines for MAPLE_STM32F1 + */ + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#ifndef BOARD_NR_GPIO_PINS // Only in MAPLE_STM32F1 + #error "Expected BOARD_NR_GPIO_PINS not found" +#endif + +#include "fastio.h" + +extern const stm32_pin_info PIN_MAP[BOARD_NR_GPIO_PINS]; + +#define NUM_DIGITAL_PINS BOARD_NR_GPIO_PINS +#define NUMBER_PINS_TOTAL BOARD_NR_GPIO_PINS +#define VALID_PIN(pin) (pin >= 0 && pin < BOARD_NR_GPIO_PINS) +#define GET_ARRAY_PIN(p) pin_t(pin_array[p].pin) +#define pwm_status(pin) PWM_PIN(pin) +#define digitalRead_mod(p) extDigitalRead(p) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3hd "), int16_t(p)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PORT(p) print_port(p) +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 21 // space needed to be pretty if not first name assigned to a pin + +// pins that will cause hang/reset/disconnect in M43 Toggle and Watch utilities +#ifndef M43_NEVER_TOUCH + #define M43_NEVER_TOUCH(Q) (Q >= 9 && Q <= 12) // SERIAL/USB pins PA9(TX) PA10(RX) +#endif + +static int8_t get_pin_mode(pin_t pin) { + return VALID_PIN(pin) ? _GET_MODE(pin) : -1; +} + +static pin_t DIGITAL_PIN_TO_ANALOG_PIN(pin_t pin) { + if (!VALID_PIN(pin)) return -1; + int8_t adc_channel = int8_t(PIN_MAP[pin].adc_channel); + #ifdef NUM_ANALOG_INPUTS + if (adc_channel >= NUM_ANALOG_INPUTS) adc_channel = ADCx; + #endif + return pin_t(adc_channel); +} + +static bool IS_ANALOG(pin_t pin) { + if (!VALID_PIN(pin)) return false; + if (PIN_MAP[pin].adc_channel != ADCx) { + #ifdef NUM_ANALOG_INPUTS + if (PIN_MAP[pin].adc_channel >= NUM_ANALOG_INPUTS) return false; + #endif + return _GET_MODE(pin) == GPIO_INPUT_ANALOG && !M43_NEVER_TOUCH(pin); + } + return false; +} + +static bool GET_PINMODE(const pin_t pin) { + return VALID_PIN(pin) && !IS_INPUT(pin); +} + +static bool GET_ARRAY_IS_DIGITAL(const int16_t array_pin) { + const pin_t pin = GET_ARRAY_PIN(array_pin); + return (!IS_ANALOG(pin) + #ifdef NUM_ANALOG_INPUTS + || PIN_MAP[pin].adc_channel >= NUM_ANALOG_INPUTS + #endif + ); +} + +#include "../../inc/MarlinConfig.h" // Allow pins/pins.h to set density + +static void pwm_details(const pin_t pin) { + if (PWM_PIN(pin)) { + timer_dev * const tdev = PIN_MAP[pin].timer_device; + const uint8_t channel = PIN_MAP[pin].timer_channel; + const char num = ( + #if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY) + tdev == &timer8 ? '8' : + tdev == &timer5 ? '5' : + #endif + tdev == &timer4 ? '4' : + tdev == &timer3 ? '3' : + tdev == &timer2 ? '2' : + tdev == &timer1 ? '1' : '?' + ); + char buffer[10]; + sprintf_P(buffer, PSTR(" TIM%c CH%c"), num, ('0' + channel)); + SERIAL_ECHO(buffer); + } +} + +static void print_port(pin_t pin) { + const char port = 'A' + char(pin >> 4); // pin div 16 + const int16_t gbit = PIN_MAP[pin].gpio_bit; + char buffer[8]; + sprintf_P(buffer, PSTR("P%c%hd "), port, gbit); + if (gbit < 10) SERIAL_CHAR(' '); + SERIAL_ECHO(buffer); +} diff --git a/src/HAL/STM32F1/sdio.cpp b/src/HAL/STM32F1/sdio.cpp new file mode 100644 index 0000000..6e41d2c --- /dev/null +++ b/src/HAL/STM32F1/sdio.cpp @@ -0,0 +1,307 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_STM32F1 + +#include + +#include "../../inc/MarlinConfig.h" // Allow pins/pins.h to set density + +#if EITHER(STM32_HIGH_DENSITY, STM32_XL_DENSITY) + +#include "sdio.h" + +SDIO_CardInfoTypeDef SdCard; + +bool SDIO_Init() { + uint32_t count = 0U; + SdCard.CardType = SdCard.CardVersion = SdCard.Class = SdCard.RelCardAdd = SdCard.BlockNbr = SdCard.BlockSize = SdCard.LogBlockNbr = SdCard.LogBlockSize = 0; + + sdio_begin(); + sdio_set_dbus_width(SDIO_CLKCR_WIDBUS_1BIT); + + dma_init(SDIO_DMA_DEV); + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + dma_set_priority(SDIO_DMA_DEV, SDIO_DMA_CHANNEL, DMA_PRIORITY_MEDIUM); + + if (!SDIO_CmdGoIdleState()) return false; + if (!SDIO_CmdGoIdleState()) return false; /* Hotplugged cards tends to miss first CMD0, so give them a second chance. */ + + SdCard.CardVersion = SDIO_CmdOperCond() ? CARD_V2_X : CARD_V1_X; + + do { + if (count++ == SDMMC_MAX_VOLT_TRIAL) return false; + SDIO_CmdAppOperCommand(SdCard.CardVersion == CARD_V2_X ? SDMMC_HIGH_CAPACITY : SDMMC_STD_CAPACITY); + } while ((SDIO_GetResponse(SDIO_RESP1) & 0x80000000) == 0); + + SdCard.CardType = (SDIO_GetResponse(SDIO_RESP1) & SDMMC_HIGH_CAPACITY) ? CARD_SDHC_SDXC : CARD_SDSC; + + if (!SDIO_CmdSendCID()) return false; + if (!SDIO_CmdSetRelAdd(&SdCard.RelCardAdd)) return false; /* Send CMD3 SET_REL_ADDR with argument 0. SD Card publishes its RCA. */ + if (!SDIO_CmdSendCSD(SdCard.RelCardAdd << 16U)) return false; + + SdCard.Class = (SDIO_GetResponse(SDIO_RESP2) >> 20U); + + if (SdCard.CardType == CARD_SDHC_SDXC) { + SdCard.LogBlockNbr = SdCard.BlockNbr = (((SDIO_GetResponse(SDIO_RESP2) & 0x0000003FU) << 26U) | ((SDIO_GetResponse(SDIO_RESP3) & 0xFFFF0000U) >> 6U)) + 1024; + SdCard.LogBlockSize = SdCard.BlockSize = 512U; + } + else { + SdCard.BlockNbr = ((((SDIO_GetResponse(SDIO_RESP2) & 0x000003FFU) << 2U ) | ((SDIO_GetResponse(SDIO_RESP3) & 0xC0000000U) >> 30U)) + 1U) * (4U << ((SDIO_GetResponse(SDIO_RESP3) & 0x00038000U) >> 15U)); + SdCard.BlockSize = 1U << ((SDIO_GetResponse(SDIO_RESP2) >> 16) & 0x0FU); + SdCard.LogBlockNbr = (SdCard.BlockNbr) * ((SdCard.BlockSize) / 512U); + SdCard.LogBlockSize = 512U; + } + + if (!SDIO_CmdSelDesel(SdCard.RelCardAdd << 16U)) return false; + if (!SDIO_CmdAppSetClearCardDetect(SdCard.RelCardAdd << 16U)) return false; + if (!SDIO_CmdAppSetBusWidth(SdCard.RelCardAdd << 16U, 2)) return false; + + sdio_set_dbus_width(SDIO_CLKCR_WIDBUS_4BIT); + sdio_set_clock(SDIO_CLOCK); + return true; +} + +bool SDIO_ReadBlock_DMA(uint32_t blockAddress, uint8_t *data) { + if (SDIO_GetCardState() != SDIO_CARD_TRANSFER) return false; + if (blockAddress >= SdCard.LogBlockNbr) return false; + if ((0x03 & (uint32_t)data)) return false; // misaligned data + + if (SdCard.CardType != CARD_SDHC_SDXC) { blockAddress *= 512U; } + + dma_setup_transfer(SDIO_DMA_DEV, SDIO_DMA_CHANNEL, &SDIO->FIFO, DMA_SIZE_32BITS, data, DMA_SIZE_32BITS, DMA_MINC_MODE); + dma_set_num_transfers(SDIO_DMA_DEV, SDIO_DMA_CHANNEL, 128); + dma_clear_isr_bits(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + dma_enable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + + sdio_setup_transfer(SDIO_DATA_TIMEOUT * (F_CPU / 1000U), 512, SDIO_BLOCKSIZE_512 | SDIO_DCTRL_DMAEN | SDIO_DCTRL_DTEN | SDIO_DIR_RX); + + if (!SDIO_CmdReadSingleBlock(blockAddress)) { + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + return false; + } + + while (!SDIO_GET_FLAG(SDIO_STA_DATAEND | SDIO_STA_TRX_ERROR_FLAGS)) { /* wait */ } + + //If there were SDIO errors, do not wait DMA. + if (SDIO->STA & SDIO_STA_TRX_ERROR_FLAGS) { + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + return false; + } + + //Wait for DMA transaction to complete + while ((DMA2_BASE->ISR & (DMA_ISR_TEIF4|DMA_ISR_TCIF4)) == 0 ) { /* wait */ } + + if (DMA2_BASE->ISR & DMA_ISR_TEIF4) { + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + return false; + } + + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + + if (SDIO->STA & SDIO_STA_RXDAVL) { + while (SDIO->STA & SDIO_STA_RXDAVL) (void)SDIO->FIFO; + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + return false; + } + + if (SDIO_GET_FLAG(SDIO_STA_TRX_ERROR_FLAGS)) { + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + return false; + } + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + return true; +} + +bool SDIO_ReadBlock(uint32_t blockAddress, uint8_t *data) { + uint32_t retries = SDIO_READ_RETRIES; + while (retries--) if (SDIO_ReadBlock_DMA(blockAddress, data)) return true; + return false; +} + +uint32_t millis(); + +bool SDIO_WriteBlock(uint32_t blockAddress, const uint8_t *data) { + if (SDIO_GetCardState() != SDIO_CARD_TRANSFER) return false; + if (blockAddress >= SdCard.LogBlockNbr) return false; + if ((0x03 & (uint32_t)data)) return false; // misaligned data + + if (SdCard.CardType != CARD_SDHC_SDXC) { blockAddress *= 512U; } + + dma_setup_transfer(SDIO_DMA_DEV, SDIO_DMA_CHANNEL, &SDIO->FIFO, DMA_SIZE_32BITS, (volatile void *) data, DMA_SIZE_32BITS, DMA_MINC_MODE | DMA_FROM_MEM); + dma_set_num_transfers(SDIO_DMA_DEV, SDIO_DMA_CHANNEL, 128); + dma_clear_isr_bits(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + dma_enable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + + if (!SDIO_CmdWriteSingleBlock(blockAddress)) { + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + return false; + } + + sdio_setup_transfer(SDIO_DATA_TIMEOUT * (F_CPU / 1000U), 512U, SDIO_BLOCKSIZE_512 | SDIO_DCTRL_DMAEN | SDIO_DCTRL_DTEN); + + while (!SDIO_GET_FLAG(SDIO_STA_DATAEND | SDIO_STA_TRX_ERROR_FLAGS)) { /* wait */ } + + dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); + + if (SDIO_GET_FLAG(SDIO_STA_TRX_ERROR_FLAGS)) { + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + return false; + } + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); + + uint32_t timeout = millis() + SDIO_WRITE_TIMEOUT; + while (timeout > millis()) { + if (SDIO_GetCardState() == SDIO_CARD_TRANSFER) { + return true; + } + } + return false; +} + +inline uint32_t SDIO_GetCardState() { return SDIO_CmdSendStatus(SdCard.RelCardAdd << 16U) ? (SDIO_GetResponse(SDIO_RESP1) >> 9U) & 0x0FU : SDIO_CARD_ERROR; } + +// No F1 board with SDIO + MSC using Maple, that I aware of... +bool SDIO_IsReady() { return true; } +uint32_t SDIO_GetCardSize() { return 0; } + +// ------------------------ +// SD Commands and Responses +// ------------------------ + +void SDIO_SendCommand(uint16_t command, uint32_t argument) { SDIO->ARG = argument; SDIO->CMD = (uint32_t)(SDIO_CMD_CPSMEN | command); } +uint8_t SDIO_GetCommandResponse() { return (uint8_t)(SDIO->RESPCMD); } +uint32_t SDIO_GetResponse(uint32_t response) { return SDIO->RESP[response]; } + +bool SDIO_CmdGoIdleState() { SDIO_SendCommand(CMD0_GO_IDLE_STATE, 0); return SDIO_GetCmdError(); } +bool SDIO_CmdSendCID() { SDIO_SendCommand(CMD2_ALL_SEND_CID, 0); return SDIO_GetCmdResp2(); } +bool SDIO_CmdSetRelAdd(uint32_t *rca) { SDIO_SendCommand(CMD3_SET_REL_ADDR, 0); return SDIO_GetCmdResp6(SDMMC_CMD_SET_REL_ADDR, rca); } +bool SDIO_CmdSelDesel(uint32_t address) { SDIO_SendCommand(CMD7_SEL_DESEL_CARD, address); return SDIO_GetCmdResp1(SDMMC_CMD_SEL_DESEL_CARD); } +bool SDIO_CmdOperCond() { SDIO_SendCommand(CMD8_HS_SEND_EXT_CSD, SDMMC_CHECK_PATTERN); return SDIO_GetCmdResp7(); } +bool SDIO_CmdSendCSD(uint32_t argument) { SDIO_SendCommand(CMD9_SEND_CSD, argument); return SDIO_GetCmdResp2(); } +bool SDIO_CmdSendStatus(uint32_t argument) { SDIO_SendCommand(CMD13_SEND_STATUS, argument); return SDIO_GetCmdResp1(SDMMC_CMD_SEND_STATUS); } +bool SDIO_CmdReadSingleBlock(uint32_t address) { SDIO_SendCommand(CMD17_READ_SINGLE_BLOCK, address); return SDIO_GetCmdResp1(SDMMC_CMD_READ_SINGLE_BLOCK); } +bool SDIO_CmdWriteSingleBlock(uint32_t address) { SDIO_SendCommand(CMD24_WRITE_SINGLE_BLOCK, address); return SDIO_GetCmdResp1(SDMMC_CMD_WRITE_SINGLE_BLOCK); } +bool SDIO_CmdAppCommand(uint32_t rsa) { SDIO_SendCommand(CMD55_APP_CMD, rsa); return SDIO_GetCmdResp1(SDMMC_CMD_APP_CMD); } + +bool SDIO_CmdAppSetBusWidth(uint32_t rsa, uint32_t argument) { + if (!SDIO_CmdAppCommand(rsa)) return false; + SDIO_SendCommand(ACMD6_APP_SD_SET_BUSWIDTH, argument); + return SDIO_GetCmdResp2(); +} + +bool SDIO_CmdAppOperCommand(uint32_t sdType) { + if (!SDIO_CmdAppCommand(0)) return false; + SDIO_SendCommand(ACMD41_SD_APP_OP_COND , SDMMC_VOLTAGE_WINDOW_SD | sdType); + return SDIO_GetCmdResp3(); +} + +bool SDIO_CmdAppSetClearCardDetect(uint32_t rsa) { + if (!SDIO_CmdAppCommand(rsa)) return false; + SDIO_SendCommand(ACMD42_SD_APP_SET_CLR_CARD_DETECT, 0); + return SDIO_GetCmdResp2(); +} + +// Wait until given flags are unset or till timeout +#define SDIO_WAIT(FLAGS) do{ \ + uint32_t count = 1 + (SDIO_CMDTIMEOUT) * ((F_CPU) / 8U / 1000U); \ + do { if (!--count) return false; } while (!SDIO_GET_FLAG(FLAGS)); \ +}while(0) + +bool SDIO_GetCmdError() { + SDIO_WAIT(SDIO_STA_CMDSENT); + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + return true; +} + +bool SDIO_GetCmdResp1(uint8_t command) { + SDIO_WAIT(SDIO_STA_CCRCFAIL | SDIO_STA_CMDREND | SDIO_STA_CTIMEOUT); + + if (SDIO_GET_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT)) { + SDIO_CLEAR_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT); + return false; + } + if (SDIO_GetCommandResponse() != command) return false; + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + return (SDIO_GetResponse(SDIO_RESP1) & SDMMC_OCR_ERRORBITS) == SDMMC_ALLZERO; +} + +bool SDIO_GetCmdResp2() { + SDIO_WAIT(SDIO_STA_CCRCFAIL | SDIO_STA_CMDREND | SDIO_STA_CTIMEOUT); + + if (SDIO_GET_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT)) { + SDIO_CLEAR_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT); + return false; + } + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + return true; +} + +bool SDIO_GetCmdResp3() { + SDIO_WAIT(SDIO_STA_CCRCFAIL | SDIO_STA_CMDREND | SDIO_STA_CTIMEOUT); + + if (SDIO_GET_FLAG(SDIO_STA_CTIMEOUT)) { + SDIO_CLEAR_FLAG(SDIO_STA_CTIMEOUT); + return false; + } + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + return true; +} + +bool SDIO_GetCmdResp6(uint8_t command, uint32_t *rca) { + SDIO_WAIT(SDIO_STA_CCRCFAIL | SDIO_STA_CMDREND | SDIO_STA_CTIMEOUT); + + if (SDIO_GET_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT)) { + SDIO_CLEAR_FLAG(SDIO_STA_CCRCFAIL | SDIO_STA_CTIMEOUT); + return false; + } + if (SDIO_GetCommandResponse() != command) return false; + + SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); + if (SDIO_GetResponse(SDIO_RESP1) & (SDMMC_R6_GENERAL_UNKNOWN_ERROR | SDMMC_R6_ILLEGAL_CMD | SDMMC_R6_COM_CRC_FAILED)) return false; + + *rca = SDIO_GetResponse(SDIO_RESP1) >> 16; + return true; +} + +bool SDIO_GetCmdResp7() { + SDIO_WAIT(SDIO_STA_CCRCFAIL | SDIO_STA_CMDREND | SDIO_STA_CTIMEOUT); + + if (SDIO_GET_FLAG(SDIO_STA_CTIMEOUT)) { + SDIO_CLEAR_FLAG(SDIO_STA_CTIMEOUT); + return false; + } + + if (SDIO_GET_FLAG(SDIO_STA_CMDREND)) { SDIO_CLEAR_FLAG(SDIO_STA_CMDREND); } + return true; +} + +#endif // STM32_HIGH_DENSITY || STM32_XL_DENSITY +#endif // ARDUINO_ARCH_STM32F1 diff --git a/src/HAL/STM32F1/sdio.h b/src/HAL/STM32F1/sdio.h new file mode 100644 index 0000000..8777299 --- /dev/null +++ b/src/HAL/STM32F1/sdio.h @@ -0,0 +1,155 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfig.h" // Allow pins/pins.h to override SDIO clock / retries + +#include +#include + +// ------------------------ +// Defines +// ------------------------ + +#define SDMMC_CMD_GO_IDLE_STATE ((uint8_t)0) /* Resets the SD memory card. */ +#define SDMMC_CMD_ALL_SEND_CID ((uint8_t)2) /* Asks any card connected to the host to send the CID numbers on the CMD line. */ +#define SDMMC_CMD_SET_REL_ADDR ((uint8_t)3) /* Asks the card to publish a new relative address (RCA). */ +#define SDMMC_CMD_SEL_DESEL_CARD ((uint8_t)7) /* Selects the card by its own relative address and gets deselected by any other address */ +#define SDMMC_CMD_HS_SEND_EXT_CSD ((uint8_t)8) /* Sends SD Memory Card interface condition, which includes host supply voltage information and asks the card whether card supports voltage. */ +#define SDMMC_CMD_SEND_CSD ((uint8_t)9) /* Addressed card sends its card specific data (CSD) on the CMD line. */ +#define SDMMC_CMD_SEND_STATUS ((uint8_t)13) /*!< Addressed card sends its status register. */ +#define SDMMC_CMD_READ_SINGLE_BLOCK ((uint8_t)17) /* Reads single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of fixed 512 bytes in case of SDHC and SDXC. */ +#define SDMMC_CMD_WRITE_SINGLE_BLOCK ((uint8_t)24) /* Writes single block of size selected by SET_BLOCKLEN in case of SDSC, and a block of fixed 512 bytes in case of SDHC and SDXC. */ +#define SDMMC_CMD_APP_CMD ((uint8_t)55) /* Indicates to the card that the next command is an application specific command rather than a standard command. */ + +#define SDMMC_ACMD_APP_SD_SET_BUSWIDTH ((uint8_t)6) /* (ACMD6) Defines the data bus width to be used for data transfer. The allowed data bus widths are given in SCR register. */ +#define SDMMC_ACMD_SD_APP_OP_COND ((uint8_t)41) /* (ACMD41) Sends host capacity support information (HCS) and asks the accessed card to send its operating condition register (OCR) content in the response on the CMD line. */ +#define SDMMC_ACMD_SD_APP_SET_CLR_CARD_DETECT ((uint8_t)42) /* (ACMD42) Connect/Disconnect the 50 KOhm pull-up resistor on CD/DAT3 (pin 1) of the card */ + +#define CMD0_GO_IDLE_STATE (uint16_t)(SDMMC_CMD_GO_IDLE_STATE | SDIO_CMD_WAIT_NO_RESP) +#define CMD2_ALL_SEND_CID (uint16_t)(SDMMC_CMD_ALL_SEND_CID | SDIO_CMD_WAIT_LONG_RESP) +#define CMD3_SET_REL_ADDR (uint16_t)(SDMMC_CMD_SET_REL_ADDR | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD7_SEL_DESEL_CARD (uint16_t)(SDMMC_CMD_SEL_DESEL_CARD | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD8_HS_SEND_EXT_CSD (uint16_t)(SDMMC_CMD_HS_SEND_EXT_CSD | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD9_SEND_CSD (uint16_t)(SDMMC_CMD_SEND_CSD | SDIO_CMD_WAIT_LONG_RESP) +#define CMD13_SEND_STATUS (uint16_t)(SDMMC_CMD_SEND_STATUS | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD17_READ_SINGLE_BLOCK (uint16_t)(SDMMC_CMD_READ_SINGLE_BLOCK | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD24_WRITE_SINGLE_BLOCK (uint16_t)(SDMMC_CMD_WRITE_SINGLE_BLOCK | SDIO_CMD_WAIT_SHORT_RESP) +#define CMD55_APP_CMD (uint16_t)(SDMMC_CMD_APP_CMD | SDIO_CMD_WAIT_SHORT_RESP) + +#define ACMD6_APP_SD_SET_BUSWIDTH (uint16_t)(SDMMC_ACMD_APP_SD_SET_BUSWIDTH | SDIO_CMD_WAIT_SHORT_RESP) +#define ACMD41_SD_APP_OP_COND (uint16_t)(SDMMC_ACMD_SD_APP_OP_COND | SDIO_CMD_WAIT_SHORT_RESP) +#define ACMD42_SD_APP_SET_CLR_CARD_DETECT (uint16_t)(SDMMC_ACMD_SD_APP_SET_CLR_CARD_DETECT | SDIO_CMD_WAIT_SHORT_RESP) + + +#define SDMMC_ALLZERO 0x00000000U +#define SDMMC_OCR_ERRORBITS 0xFDFFE008U + +#define SDMMC_R6_GENERAL_UNKNOWN_ERROR 0x00002000U +#define SDMMC_R6_ILLEGAL_CMD 0x00004000U +#define SDMMC_R6_COM_CRC_FAILED 0x00008000U + +#define SDMMC_VOLTAGE_WINDOW_SD 0x80100000U +#define SDMMC_HIGH_CAPACITY 0x40000000U +#define SDMMC_STD_CAPACITY 0x00000000U +#define SDMMC_CHECK_PATTERN 0x000001AAU + +#define SDIO_TRANSFER_MODE_BLOCK 0x00000000U +#define SDIO_DPSM_ENABLE 0x00000001U +#define SDIO_TRANSFER_DIR_TO_CARD 0x00000000U +#define SDIO_DATABLOCK_SIZE_512B 0x00000090U +#define SDIO_TRANSFER_DIR_TO_SDIO 0x00000100U +#define SDIO_DMA_ENABLE 0x00001000U + +#define CARD_V1_X 0x00000000U +#define CARD_V2_X 0x00000001U +#define CARD_SDSC 0x00000000U +#define CARD_SDHC_SDXC 0x00000001U + +#define SDIO_RESP1 0 +#define SDIO_RESP2 1 +#define SDIO_RESP3 2 +#define SDIO_RESP4 3 + +#define SDIO_GET_FLAG(__FLAG__) !!((SDIO->STA) & (__FLAG__)) +#define SDIO_CLEAR_FLAG(__FLAG__) (SDIO->ICR = (__FLAG__)) + +#define SDMMC_MAX_VOLT_TRIAL 0x00000FFFU +#define SDIO_CARD_TRANSFER 0x00000004U /* Card is in transfer state */ +#define SDIO_CARD_ERROR 0x000000FFU /* Card response Error */ +#define SDIO_CMDTIMEOUT 200U /* Command send and response timeout */ +#define SDIO_DATA_TIMEOUT 100U /* Read data transfer timeout */ +#define SDIO_WRITE_TIMEOUT 200U /* Write data transfer timeout */ + +#ifndef SDIO_CLOCK + #define SDIO_CLOCK 18000000 /* 18 MHz */ +#endif + +#ifndef SDIO_READ_RETRIES + #define SDIO_READ_RETRIES 3 +#endif + +// ------------------------ +// Types +// ------------------------ + +typedef struct { + uint32_t CardType; // Card Type + uint32_t CardVersion; // Card version + uint32_t Class; // Class of the card class + uint32_t RelCardAdd; // Relative Card Address + uint32_t BlockNbr; // Card Capacity in blocks + uint32_t BlockSize; // One block size in bytes + uint32_t LogBlockNbr; // Card logical Capacity in blocks + uint32_t LogBlockSize; // Logical block size in bytes +} SDIO_CardInfoTypeDef; + +// ------------------------ +// Public functions +// ------------------------ + +inline uint32_t SDIO_GetCardState(); + +bool SDIO_CmdGoIdleState(); +bool SDIO_CmdSendCID(); +bool SDIO_CmdSetRelAdd(uint32_t *rca); +bool SDIO_CmdSelDesel(uint32_t address); +bool SDIO_CmdOperCond(); +bool SDIO_CmdSendCSD(uint32_t argument); +bool SDIO_CmdSendStatus(uint32_t argument); +bool SDIO_CmdReadSingleBlock(uint32_t address); +bool SDIO_CmdWriteSingleBlock(uint32_t address); +bool SDIO_CmdAppCommand(uint32_t rsa); + +bool SDIO_CmdAppSetBusWidth(uint32_t rsa, uint32_t argument); +bool SDIO_CmdAppOperCommand(uint32_t sdType); +bool SDIO_CmdAppSetClearCardDetect(uint32_t rsa); + +void SDIO_SendCommand(uint16_t command, uint32_t argument); +uint8_t SDIO_GetCommandResponse(); +uint32_t SDIO_GetResponse(uint32_t response); +bool SDIO_GetCmdError(); +bool SDIO_GetCmdResp1(uint8_t command); +bool SDIO_GetCmdResp2(); +bool SDIO_GetCmdResp3(); +bool SDIO_GetCmdResp6(uint8_t command, uint32_t *rca); +bool SDIO_GetCmdResp7(); diff --git a/src/HAL/STM32F1/spi_pins.h b/src/HAL/STM32F1/spi_pins.h new file mode 100644 index 0000000..3d3c8f8 --- /dev/null +++ b/src/HAL/STM32F1/spi_pins.h @@ -0,0 +1,57 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + */ + +/** + * STM32F1 Default SPI Pins + * + * SS SCK MISO MOSI + * +-----------------------------+ + * SPI1 | PA4 PA5 PA6 PA7 | + * SPI2 | PB12 PB13 PB14 PB15 | + * SPI3 | PA15 PB3 PB4 PB5 | + * +-----------------------------+ + * Any pin can be used for Chip Select (SD_SS_PIN) + * SPI1 is enabled by default + */ +#ifndef SD_SCK_PIN + #define SD_SCK_PIN PA5 +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN PA6 +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN PA7 +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN PA4 +#endif +#undef SDSS +#define SDSS SD_SS_PIN + +#ifndef SPI_DEVICE + #define SPI_DEVICE 1 +#endif diff --git a/src/HAL/STM32F1/tft/tft_fsmc.cpp b/src/HAL/STM32F1/tft/tft_fsmc.cpp new file mode 100644 index 0000000..5b52fb4 --- /dev/null +++ b/src/HAL/STM32F1/tft/tft_fsmc.cpp @@ -0,0 +1,237 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_FSMC_TFT + +#include "tft_fsmc.h" +#include +#include +#include + +LCD_CONTROLLER_TypeDef *TFT_FSMC::LCD; + +/** + * FSMC LCD IO + */ +#define __ASM __asm +#define __STATIC_INLINE static inline + +__attribute__((always_inline)) __STATIC_INLINE void __DSB() { + __ASM volatile ("dsb 0xF":::"memory"); +} + +#define FSMC_CS_NE1 PD7 + +#if ENABLED(STM32_XL_DENSITY) + #define FSMC_CS_NE2 PG9 + #define FSMC_CS_NE3 PG10 + #define FSMC_CS_NE4 PG12 + + #define FSMC_RS_A0 PF0 + #define FSMC_RS_A1 PF1 + #define FSMC_RS_A2 PF2 + #define FSMC_RS_A3 PF3 + #define FSMC_RS_A4 PF4 + #define FSMC_RS_A5 PF5 + #define FSMC_RS_A6 PF12 + #define FSMC_RS_A7 PF13 + #define FSMC_RS_A8 PF14 + #define FSMC_RS_A9 PF15 + #define FSMC_RS_A10 PG0 + #define FSMC_RS_A11 PG1 + #define FSMC_RS_A12 PG2 + #define FSMC_RS_A13 PG3 + #define FSMC_RS_A14 PG4 + #define FSMC_RS_A15 PG5 +#endif + +#define FSMC_RS_A16 PD11 +#define FSMC_RS_A17 PD12 +#define FSMC_RS_A18 PD13 +#define FSMC_RS_A19 PE3 +#define FSMC_RS_A20 PE4 +#define FSMC_RS_A21 PE5 +#define FSMC_RS_A22 PE6 +#define FSMC_RS_A23 PE2 + +#if ENABLED(STM32_XL_DENSITY) + #define FSMC_RS_A24 PG13 + #define FSMC_RS_A25 PG14 +#endif + +/* Timing configuration */ +#define FSMC_ADDRESS_SETUP_TIME 15 // AddressSetupTime +#define FSMC_DATA_SETUP_TIME 15 // DataSetupTime + +static uint8_t fsmcInit = 0; +void TFT_FSMC::Init() { + uint8_t cs = FSMC_CS_PIN, rs = FSMC_RS_PIN; + uint32_t controllerAddress; + + #if ENABLED(LCD_USE_DMA_FSMC) + dma_init(FSMC_DMA_DEV); + dma_disable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL); + dma_set_priority(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, DMA_PRIORITY_MEDIUM); + #endif + + struct fsmc_nor_psram_reg_map* fsmcPsramRegion; + + if (fsmcInit) return; + fsmcInit = 1; + + switch (cs) { + case FSMC_CS_NE1: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION1; fsmcPsramRegion = FSMC_NOR_PSRAM1_BASE; break; + #if ENABLED(STM32_XL_DENSITY) + case FSMC_CS_NE2: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION2; fsmcPsramRegion = FSMC_NOR_PSRAM2_BASE; break; + case FSMC_CS_NE3: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION3; fsmcPsramRegion = FSMC_NOR_PSRAM3_BASE; break; + case FSMC_CS_NE4: controllerAddress = (uint32_t)FSMC_NOR_PSRAM_REGION4; fsmcPsramRegion = FSMC_NOR_PSRAM4_BASE; break; + #endif + default: return; + } + + #define _ORADDR(N) controllerAddress |= (_BV32(N) - 2) + + switch (rs) { + #if ENABLED(STM32_XL_DENSITY) + case FSMC_RS_A0: _ORADDR( 1); break; + case FSMC_RS_A1: _ORADDR( 2); break; + case FSMC_RS_A2: _ORADDR( 3); break; + case FSMC_RS_A3: _ORADDR( 4); break; + case FSMC_RS_A4: _ORADDR( 5); break; + case FSMC_RS_A5: _ORADDR( 6); break; + case FSMC_RS_A6: _ORADDR( 7); break; + case FSMC_RS_A7: _ORADDR( 8); break; + case FSMC_RS_A8: _ORADDR( 9); break; + case FSMC_RS_A9: _ORADDR(10); break; + case FSMC_RS_A10: _ORADDR(11); break; + case FSMC_RS_A11: _ORADDR(12); break; + case FSMC_RS_A12: _ORADDR(13); break; + case FSMC_RS_A13: _ORADDR(14); break; + case FSMC_RS_A14: _ORADDR(15); break; + case FSMC_RS_A15: _ORADDR(16); break; + #endif + case FSMC_RS_A16: _ORADDR(17); break; + case FSMC_RS_A17: _ORADDR(18); break; + case FSMC_RS_A18: _ORADDR(19); break; + case FSMC_RS_A19: _ORADDR(20); break; + case FSMC_RS_A20: _ORADDR(21); break; + case FSMC_RS_A21: _ORADDR(22); break; + case FSMC_RS_A22: _ORADDR(23); break; + case FSMC_RS_A23: _ORADDR(24); break; + #if ENABLED(STM32_XL_DENSITY) + case FSMC_RS_A24: _ORADDR(25); break; + case FSMC_RS_A25: _ORADDR(26); break; + #endif + default: return; + } + + rcc_clk_enable(RCC_FSMC); + + gpio_set_mode(GPIOD, 14, GPIO_AF_OUTPUT_PP); // FSMC_D00 + gpio_set_mode(GPIOD, 15, GPIO_AF_OUTPUT_PP); // FSMC_D01 + gpio_set_mode(GPIOD, 0, GPIO_AF_OUTPUT_PP); // FSMC_D02 + gpio_set_mode(GPIOD, 1, GPIO_AF_OUTPUT_PP); // FSMC_D03 + gpio_set_mode(GPIOE, 7, GPIO_AF_OUTPUT_PP); // FSMC_D04 + gpio_set_mode(GPIOE, 8, GPIO_AF_OUTPUT_PP); // FSMC_D05 + gpio_set_mode(GPIOE, 9, GPIO_AF_OUTPUT_PP); // FSMC_D06 + gpio_set_mode(GPIOE, 10, GPIO_AF_OUTPUT_PP); // FSMC_D07 + gpio_set_mode(GPIOE, 11, GPIO_AF_OUTPUT_PP); // FSMC_D08 + gpio_set_mode(GPIOE, 12, GPIO_AF_OUTPUT_PP); // FSMC_D09 + gpio_set_mode(GPIOE, 13, GPIO_AF_OUTPUT_PP); // FSMC_D10 + gpio_set_mode(GPIOE, 14, GPIO_AF_OUTPUT_PP); // FSMC_D11 + gpio_set_mode(GPIOE, 15, GPIO_AF_OUTPUT_PP); // FSMC_D12 + gpio_set_mode(GPIOD, 8, GPIO_AF_OUTPUT_PP); // FSMC_D13 + gpio_set_mode(GPIOD, 9, GPIO_AF_OUTPUT_PP); // FSMC_D14 + gpio_set_mode(GPIOD, 10, GPIO_AF_OUTPUT_PP); // FSMC_D15 + + gpio_set_mode(GPIOD, 4, GPIO_AF_OUTPUT_PP); // FSMC_NOE + gpio_set_mode(GPIOD, 5, GPIO_AF_OUTPUT_PP); // FSMC_NWE + + gpio_set_mode(PIN_MAP[cs].gpio_device, PIN_MAP[cs].gpio_bit, GPIO_AF_OUTPUT_PP); //FSMC_CS_NEx + gpio_set_mode(PIN_MAP[rs].gpio_device, PIN_MAP[rs].gpio_bit, GPIO_AF_OUTPUT_PP); //FSMC_RS_Ax + + fsmcPsramRegion->BCR = FSMC_BCR_WREN | FSMC_BCR_MTYP_SRAM | FSMC_BCR_MWID_16BITS | FSMC_BCR_MBKEN; + fsmcPsramRegion->BTR = (FSMC_DATA_SETUP_TIME << 8) | FSMC_ADDRESS_SETUP_TIME; + + afio_remap(AFIO_REMAP_FSMC_NADV); + + LCD = (LCD_CONTROLLER_TypeDef*)controllerAddress; +} + +void TFT_FSMC::Transmit(uint16_t Data) { + LCD->RAM = Data; + __DSB(); +} + +void TFT_FSMC::WriteReg(uint16_t Reg) { + LCD->REG = Reg; + __DSB(); +} + +uint32_t TFT_FSMC::GetID() { + uint32_t id; + WriteReg(0x0000); + id = LCD->RAM; + + if (id == 0) + id = ReadID(LCD_READ_ID); + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = ReadID(LCD_READ_ID4); + if ((id & 0xFF00) == 0 && (id & 0xFF) != 0) + id = ReadID(LCD_READ_ID4); + return id; +} + + uint32_t TFT_FSMC::ReadID(uint16_t Reg) { + uint32_t id; + WriteReg(Reg); + id = LCD->RAM; // dummy read + id = Reg << 24; + id |= (LCD->RAM & 0x00FF) << 16; + id |= (LCD->RAM & 0x00FF) << 8; + id |= LCD->RAM & 0x00FF; + return id; + } + +bool TFT_FSMC::isBusy() { + return false; +} + +void TFT_FSMC::Abort() { + +} + +void TFT_FSMC::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + #if defined(FSMC_DMA_DEV) && defined(FSMC_DMA_CHANNEL) + dma_setup_transfer(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, Data, DMA_SIZE_16BITS, &LCD->RAM, DMA_SIZE_16BITS, DMA_MEM_2_MEM | MemoryIncrease); + dma_set_num_transfers(FSMC_DMA_DEV, FSMC_DMA_CHANNEL, Count); + dma_clear_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL); + dma_enable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL); + + while ((dma_get_isr_bits(FSMC_DMA_DEV, FSMC_DMA_CHANNEL) & 0x0A) == 0) {}; + dma_disable(FSMC_DMA_DEV, FSMC_DMA_CHANNEL); + #endif +} + +#endif // HAS_FSMC_TFT diff --git a/src/HAL/STM32F1/tft/tft_fsmc.h b/src/HAL/STM32F1/tft/tft_fsmc.h new file mode 100644 index 0000000..d9ee1f4 --- /dev/null +++ b/src/HAL/STM32F1/tft/tft_fsmc.h @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#include + +#define DATASIZE_8BIT DMA_SIZE_8BITS +#define DATASIZE_16BIT DMA_SIZE_16BITS +#define TFT_IO_DRIVER TFT_FSMC + +typedef struct { + __IO uint16_t REG; + __IO uint16_t RAM; +} LCD_CONTROLLER_TypeDef; + +class TFT_FSMC { + private: + static LCD_CONTROLLER_TypeDef *LCD; + + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + + public: + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATASIZE_16BIT) {}; + static void DataTransferEnd() {}; + + static void WriteData(uint16_t Data) { Transmit(Data); } + static void WriteReg(uint16_t Reg); + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_PINC_MODE, Data, Count); } + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_CIRC_MODE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_CIRC_MODE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; diff --git a/src/HAL/STM32F1/tft/tft_spi.cpp b/src/HAL/STM32F1/tft/tft_spi.cpp new file mode 100644 index 0000000..f447cec --- /dev/null +++ b/src/HAL/STM32F1/tft/tft_spi.cpp @@ -0,0 +1,129 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_SPI_TFT + +#include "tft_spi.h" + +SPIClass TFT_SPI::SPIx(1); + +void TFT_SPI::Init() { + #if PIN_EXISTS(TFT_RESET) + OUT_WRITE(TFT_RESET_PIN, HIGH); + delay(100); + #endif + + #if PIN_EXISTS(TFT_BACKLIGHT) + OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); + #endif + + OUT_WRITE(TFT_DC_PIN, HIGH); + OUT_WRITE(TFT_CS_PIN, HIGH); + + /** + * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz + * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1 + * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2 + */ + #if SPI_DEVICE == 1 + #define SPI_CLOCK_MAX SPI_CLOCK_DIV4 + #else + #define SPI_CLOCK_MAX SPI_CLOCK_DIV2 + #endif + uint8_t clock; + uint8_t spiRate = SPI_FULL_SPEED; + switch (spiRate) { + case SPI_FULL_SPEED: clock = SPI_CLOCK_MAX ; break; + case SPI_HALF_SPEED: clock = SPI_CLOCK_DIV4 ; break; + case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8 ; break; + case SPI_EIGHTH_SPEED: clock = SPI_CLOCK_DIV16; break; + case SPI_SPEED_5: clock = SPI_CLOCK_DIV32; break; + case SPI_SPEED_6: clock = SPI_CLOCK_DIV64; break; + default: clock = SPI_CLOCK_DIV2; // Default from the SPI library + } + SPIx.setModule(1); + SPIx.setClockDivider(clock); + SPIx.setBitOrder(MSBFIRST); + SPIx.setDataMode(SPI_MODE0); +} + +void TFT_SPI::DataTransferBegin(uint16_t DataSize) { + SPIx.setDataSize(DataSize); + SPIx.begin(); + OUT_WRITE(TFT_CS_PIN, LOW); +} + +#ifdef TFT_DEFAULT_DRIVER + #include "../../../lcd/tft_io/tft_ids.h" +#endif + +uint32_t TFT_SPI::GetID() { + uint32_t id; + id = ReadID(LCD_READ_ID); + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) { + id = ReadID(LCD_READ_ID4); + #ifdef TFT_DEFAULT_DRIVER + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = TFT_DEFAULT_DRIVER; + #endif + } + return id; +} + +uint32_t TFT_SPI::ReadID(uint16_t Reg) { + #if !PIN_EXISTS(TFT_MISO) + return 0; + #else + uint8_t d = 0; + uint32_t data = 0; + SPIx.setClockDivider(SPI_CLOCK_DIV16); + DataTransferBegin(DATASIZE_8BIT); + WriteReg(Reg); + + LOOP_L_N(i, 4) { + SPIx.read((uint8_t*)&d, 1); + data = (data << 8) | d; + } + + DataTransferEnd(); + SPIx.setClockDivider(SPI_CLOCK_MAX); + + return data >> 7; + #endif +} + +bool TFT_SPI::isBusy() { return false; } + +void TFT_SPI::Abort() { DataTransferEnd(); } + +void TFT_SPI::Transmit(uint16_t Data) { SPIx.send(Data); } + +void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + DataTransferBegin(); + OUT_WRITE(TFT_DC_PIN, HIGH); + SPIx.dmaSend(Data, Count, MemoryIncrease == DMA_MINC_ENABLE); + DataTransferEnd(); +} + +#endif // HAS_SPI_TFT diff --git a/src/HAL/STM32F1/tft/tft_spi.h b/src/HAL/STM32F1/tft/tft_spi.h new file mode 100644 index 0000000..da9a8e0 --- /dev/null +++ b/src/HAL/STM32F1/tft/tft_spi.h @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#include + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT DATA_SIZE_8BIT +#define DATASIZE_16BIT DATA_SIZE_16BIT +#define TFT_IO_DRIVER TFT_SPI + +#define DMA_MINC_ENABLE 1 +#define DMA_MINC_DISABLE 0 + +class TFT_SPI { +private: + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + +public: + static SPIClass SPIx; + + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATA_SIZE_16BIT); + static void DataTransferEnd() { WRITE(TFT_CS_PIN, HIGH); SPIx.end(); }; + static void DataTransferAbort(); + + static void WriteData(uint16_t Data) { Transmit(Data); } + static void WriteReg(uint16_t Reg) { WRITE(TFT_A0_PIN, LOW); Transmit(Reg); WRITE(TFT_A0_PIN, HIGH); } + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_MINC_ENABLE, Data, Count); } + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_MINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; diff --git a/src/HAL/STM32F1/tft/xpt2046.cpp b/src/HAL/STM32F1/tft/xpt2046.cpp new file mode 100644 index 0000000..ac9ad07 --- /dev/null +++ b/src/HAL/STM32F1/tft/xpt2046.cpp @@ -0,0 +1,144 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS + +#include "xpt2046.h" +#include + +uint16_t delta(uint16_t a, uint16_t b) { return a > b ? a - b : b - a; } + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include + + SPIClass XPT2046::SPIx(TOUCH_BUTTONS_HW_SPI_DEVICE); + + static void touch_spi_init(uint8_t spiRate) { + /** + * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz + * STM32F1 has 3 SPI ports, SPI1 in APB2, SPI2/SPI3 in APB1 + * so the minimum prescale of SPI1 is DIV4, SPI2/SPI3 is DIV2 + */ + uint8_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = SPI_CLOCK_DIV4; break; + case SPI_HALF_SPEED: clock = SPI_CLOCK_DIV4; break; + case SPI_QUARTER_SPEED: clock = SPI_CLOCK_DIV8; break; + case SPI_EIGHTH_SPEED: clock = SPI_CLOCK_DIV16; break; + case SPI_SPEED_5: clock = SPI_CLOCK_DIV32; break; + case SPI_SPEED_6: clock = SPI_CLOCK_DIV64; break; + default: clock = SPI_CLOCK_DIV2; // Default from the SPI library + } + XPT2046::SPIx.setModule(TOUCH_BUTTONS_HW_SPI_DEVICE); + XPT2046::SPIx.setClockDivider(clock); + XPT2046::SPIx.setBitOrder(MSBFIRST); + XPT2046::SPIx.setDataMode(SPI_MODE0); + } +#endif // TOUCH_BUTTONS_HW_SPI + +void XPT2046::Init() { + SET_INPUT(TOUCH_MISO_PIN); + SET_OUTPUT(TOUCH_MOSI_PIN); + SET_OUTPUT(TOUCH_SCK_PIN); + OUT_WRITE(TOUCH_CS_PIN, HIGH); + + #if PIN_EXISTS(TOUCH_INT) + // Optional Pendrive interrupt pin + SET_INPUT(TOUCH_INT_PIN); + #endif + + TERN_(TOUCH_BUTTONS_HW_SPI, touch_spi_init(SPI_SPEED_6)); + + // Read once to enable pendrive status pin + getRawData(XPT2046_X); +} + +bool XPT2046::isTouched() { + return isBusy() ? false : ( + #if PIN_EXISTS(TOUCH_INT) + READ(TOUCH_INT_PIN) != HIGH + #else + getRawData(XPT2046_Z1) >= XPT2046_Z1_THRESHOLD + #endif + ); +} + +bool XPT2046::getRawPoint(int16_t *x, int16_t *y) { + if (isBusy()) return false; + if (!isTouched()) return false; + *x = getRawData(XPT2046_X); + *y = getRawData(XPT2046_Y); + return isTouched(); +} + +uint16_t XPT2046::getRawData(const XPTCoordinate coordinate) { + uint16_t data[3]; + + DataTransferBegin(); + TERN_(TOUCH_BUTTONS_HW_SPI, SPIx.begin()); + + for (uint16_t i = 0; i < 3 ; i++) { + IO(coordinate); + data[i] = (IO() << 4) | (IO() >> 4); + } + + TERN_(TOUCH_BUTTONS_HW_SPI, SPIx.end()); + DataTransferEnd(); + + uint16_t delta01 = delta(data[0], data[1]), + delta02 = delta(data[0], data[2]), + delta12 = delta(data[1], data[2]); + + if (delta01 > delta02 || delta01 > delta12) + data[delta02 > delta12 ? 0 : 1] = data[2]; + + return (data[0] + data[1]) >> 1; +} + +uint16_t XPT2046::IO(uint16_t data) { + return TERN(TOUCH_BUTTONS_HW_SPI, HardwareIO, SoftwareIO)(data); +} + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + uint16_t XPT2046::HardwareIO(uint16_t data) { + uint16_t result = SPIx.transfer(data); + return result; + } +#endif + +uint16_t XPT2046::SoftwareIO(uint16_t data) { + uint16_t result = 0; + + for (uint8_t j = 0x80; j; j >>= 1) { + WRITE(TOUCH_SCK_PIN, LOW); + WRITE(TOUCH_MOSI_PIN, data & j ? HIGH : LOW); + if (READ(TOUCH_MISO_PIN)) result |= j; + WRITE(TOUCH_SCK_PIN, HIGH); + } + WRITE(TOUCH_SCK_PIN, LOW); + + return result; +} + +#endif // HAS_TFT_XPT2046 diff --git a/src/HAL/STM32F1/tft/xpt2046.h b/src/HAL/STM32F1/tft/xpt2046.h new file mode 100644 index 0000000..7c456cf --- /dev/null +++ b/src/HAL/STM32F1/tft/xpt2046.h @@ -0,0 +1,83 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include +#endif + +#ifndef TOUCH_MISO_PIN + #define TOUCH_MISO_PIN SD_MISO_PIN +#endif +#ifndef TOUCH_MOSI_PIN + #define TOUCH_MOSI_PIN SD_MOSI_PIN +#endif +#ifndef TOUCH_SCK_PIN + #define TOUCH_SCK_PIN SD_SCK_PIN +#endif +#ifndef TOUCH_CS_PIN + #define TOUCH_CS_PIN SD_SS_PIN +#endif +#ifndef TOUCH_INT_PIN + #define TOUCH_INT_PIN -1 +#endif + +#define XPT2046_DFR_MODE 0x00 +#define XPT2046_SER_MODE 0x04 +#define XPT2046_CONTROL 0x80 + +enum XPTCoordinate : uint8_t { + XPT2046_X = 0x10 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Y = 0x50 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z1 = 0x30 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, +}; + +#ifndef XPT2046_Z1_THRESHOLD + #define XPT2046_Z1_THRESHOLD 10 +#endif + +class XPT2046 { +private: + static bool isBusy() { return false; } + + static uint16_t getRawData(const XPTCoordinate coordinate); + static bool isTouched(); + + static void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static uint16_t HardwareIO(uint16_t data); + #endif + static uint16_t SoftwareIO(uint16_t data); + static uint16_t IO(uint16_t data = 0); + +public: + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static SPIClass SPIx; + #endif + + static void Init(); + static bool getRawPoint(int16_t *x, int16_t *y); +}; diff --git a/src/HAL/STM32F1/timers.cpp b/src/HAL/STM32F1/timers.cpp new file mode 100644 index 0000000..112c730 --- /dev/null +++ b/src/HAL/STM32F1/timers.cpp @@ -0,0 +1,183 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + */ + +#ifdef __STM32F1__ + +#include "../../inc/MarlinConfig.h" + +// ------------------------ +// Local defines +// ------------------------ + +// ------------------------ +// Public functions +// ------------------------ + +/** + * Timer_clock1: Prescaler 2 -> 36 MHz + * Timer_clock2: Prescaler 8 -> 9 MHz + * Timer_clock3: Prescaler 32 -> 2.25 MHz + * Timer_clock4: Prescaler 128 -> 562.5 kHz + */ + +/** + * TODO: Calculate Timer prescale value, so we get the 32bit to adjust + */ + +void HAL_timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority) { + nvic_irq_num irq_num; + switch (timer_num) { + case 1: irq_num = NVIC_TIMER1_CC; break; + case 2: irq_num = NVIC_TIMER2; break; + case 3: irq_num = NVIC_TIMER3; break; + case 4: irq_num = NVIC_TIMER4; break; + case 5: irq_num = NVIC_TIMER5; break; + #ifdef STM32_HIGH_DENSITY + // 6 & 7 are basic timers, avoid them + case 8: irq_num = NVIC_TIMER8_CC; break; + #endif + default: + /** + * This should never happen. Add a Sanitycheck for timer number. + * Should be a general timer since basic timers have no CC channels. + */ + return; + } + + nvic_irq_set_priority(irq_num, priority); +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + /** + * Give the Stepper ISR a higher priority (lower number) + * so it automatically preempts the Temperature ISR. + */ + + switch (timer_num) { + case MF_TIMER_STEP: + timer_pause(STEP_TIMER_DEV); + timer_set_mode(STEP_TIMER_DEV, STEP_TIMER_CHAN, TIMER_OUTPUT_COMPARE); // counter + timer_set_count(STEP_TIMER_DEV, 0); + timer_set_prescaler(STEP_TIMER_DEV, (uint16_t)(STEPPER_TIMER_PRESCALE - 1)); + timer_set_reload(STEP_TIMER_DEV, 0xFFFF); + timer_oc_set_mode(STEP_TIMER_DEV, STEP_TIMER_CHAN, TIMER_OC_MODE_FROZEN, TIMER_OC_NO_PRELOAD); // no output pin change + timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (STEPPER_TIMER_RATE) / frequency)); + timer_no_ARR_preload_ARPE(STEP_TIMER_DEV); // Need to be sure no preload on ARR register + timer_attach_interrupt(STEP_TIMER_DEV, STEP_TIMER_CHAN, stepTC_Handler); + HAL_timer_set_interrupt_priority(MF_TIMER_STEP, STEP_TIMER_IRQ_PRIO); + timer_generate_update(STEP_TIMER_DEV); + timer_resume(STEP_TIMER_DEV); + break; + case MF_TIMER_TEMP: + timer_pause(TEMP_TIMER_DEV); + timer_set_mode(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, TIMER_OUTPUT_COMPARE); + timer_set_count(TEMP_TIMER_DEV, 0); + timer_set_prescaler(TEMP_TIMER_DEV, (uint16_t)(TEMP_TIMER_PRESCALE - 1)); + timer_set_reload(TEMP_TIMER_DEV, 0xFFFF); + timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (F_CPU) / (TEMP_TIMER_PRESCALE) / frequency)); + timer_attach_interrupt(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, tempTC_Handler); + HAL_timer_set_interrupt_priority(MF_TIMER_TEMP, TEMP_TIMER_IRQ_PRIO); + timer_generate_update(TEMP_TIMER_DEV); + timer_resume(TEMP_TIMER_DEV); + break; + } +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: ENABLE_STEPPER_DRIVER_INTERRUPT(); break; + case MF_TIMER_TEMP: ENABLE_TEMPERATURE_INTERRUPT(); break; + } +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: DISABLE_STEPPER_DRIVER_INTERRUPT(); break; + case MF_TIMER_TEMP: DISABLE_TEMPERATURE_INTERRUPT(); break; + } +} + +static inline bool HAL_timer_irq_enabled(const timer_dev * const dev, const uint8_t interrupt) { + return bool(*bb_perip(&(dev->regs).gen->DIER, interrupt)); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return HAL_timer_irq_enabled(STEP_TIMER_DEV, STEP_TIMER_CHAN); + case MF_TIMER_TEMP: return HAL_timer_irq_enabled(TEMP_TIMER_DEV, TEMP_TIMER_CHAN); + } + return false; +} + +timer_dev* HAL_get_timer_dev(int number) { + switch (number) { + #if STM32_HAVE_TIMER(1) + case 1: return &timer1; + #endif + #if STM32_HAVE_TIMER(2) + case 2: return &timer2; + #endif + #if STM32_HAVE_TIMER(3) + case 3: return &timer3; + #endif + #if STM32_HAVE_TIMER(4) + case 4: return &timer4; + #endif + #if STM32_HAVE_TIMER(5) + case 5: return &timer5; + #endif + #if STM32_HAVE_TIMER(6) + case 6: return &timer6; + #endif + #if STM32_HAVE_TIMER(7) + case 7: return &timer7; + #endif + #if STM32_HAVE_TIMER(8) + case 8: return &timer8; + #endif + #if STM32_HAVE_TIMER(9) + case 9: return &timer9; + #endif + #if STM32_HAVE_TIMER(10) + case 10: return &timer10; + #endif + #if STM32_HAVE_TIMER(11) + case 11: return &timer11; + #endif + #if STM32_HAVE_TIMER(12) + case 12: return &timer12; + #endif + #if STM32_HAVE_TIMER(13) + case 13: return &timer13; + #endif + #if STM32_HAVE_TIMER(14) + case 14: return &timer14; + #endif + default: return nullptr; + } +} + +#endif // __STM32F1__ diff --git a/src/HAL/STM32F1/timers.h b/src/HAL/STM32F1/timers.h new file mode 100644 index 0000000..0cd807f --- /dev/null +++ b/src/HAL/STM32F1/timers.h @@ -0,0 +1,201 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) + */ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include + +// ------------------------ +// Defines +// ------------------------ + +/** + * TODO: Check and confirm what timer we will use for each Temps and stepper driving. + * We should probable drive temps with PWM. + */ + +typedef uint16_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFF + +#define HAL_TIMER_RATE uint32_t(F_CPU) // frequency of timers peripherals + +#ifndef STEP_TIMER_CHAN + #define STEP_TIMER_CHAN 1 // Channel of the timer to use for compare and interrupts +#endif +#ifndef TEMP_TIMER_CHAN + #define TEMP_TIMER_CHAN 1 // Channel of the timer to use for compare and interrupts +#endif + +/** + * Note: Timers may be used by platforms and libraries + * + * FAN PWMs: + * With FAN_SOFT_PWM disabled the Temperature class uses + * FANx_PIN timers to generate FAN PWM signals. + * + * Speaker: + * When SPEAKER is enabled, one timer is allocated by maple/tone.cpp. + * - If BEEPER_PIN has a timer channel (and USE_PIN_TIMER is + * defined in tone.cpp) it uses the pin's own timer. + * - Otherwise it uses Timer 8 on boards with STM32_HIGH_DENSITY + * or Timer 4 on other boards. + */ +#ifndef MF_TIMER_STEP + #if defined(MCU_STM32F103CB) || defined(MCU_STM32F103C8) + #define MF_TIMER_STEP 4 // For C8/CB boards, use timer 4 + #else + #define MF_TIMER_STEP 5 // for other boards, five is fine. + #endif +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 2 // Timer Index for Temperature + //#define MF_TIMER_TEMP 4 // 2->4, Timer 2 for Stepper Current PWM +#endif + +#if MB(BTT_SKR_MINI_E3_V1_0, BTT_SKR_E3_DIP, BTT_SKR_MINI_E3_V1_2, MKS_ROBIN_LITE, MKS_ROBIN_E3D, MKS_ROBIN_E3) + // SKR Mini E3 boards use PA8 as FAN_PIN, so TIMER 1 is used for Fan PWM. + #ifdef STM32_HIGH_DENSITY + #define MF_TIMER_SERVO0 8 // tone.cpp uses Timer 4 + #else + #define MF_TIMER_SERVO0 3 // tone.cpp uses Timer 8 + #endif +#else + #define MF_TIMER_SERVO0 1 // SERVO0 or BLTOUCH +#endif + +#define STEP_TIMER_IRQ_PRIO 2 +#define TEMP_TIMER_IRQ_PRIO 3 +#define SERVO0_TIMER_IRQ_PRIO 1 + +#define TEMP_TIMER_PRESCALE 1000 // prescaler for setting Temp timer, 72Khz +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_PRESCALE 18 // prescaler for setting stepper timer, 4Mhz +#define STEPPER_TIMER_RATE (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) // frequency of stepper timer +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +timer_dev* HAL_get_timer_dev(int number); +#define TIMER_DEV(num) HAL_get_timer_dev(num) +#define STEP_TIMER_DEV TIMER_DEV(MF_TIMER_STEP) +#define TEMP_TIMER_DEV TIMER_DEV(MF_TIMER_TEMP) + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() timer_enable_irq(STEP_TIMER_DEV, STEP_TIMER_CHAN) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() timer_disable_irq(STEP_TIMER_DEV, STEP_TIMER_CHAN) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() timer_enable_irq(TEMP_TIMER_DEV, TEMP_TIMER_CHAN) +#define DISABLE_TEMPERATURE_INTERRUPT() timer_disable_irq(TEMP_TIMER_DEV, TEMP_TIMER_CHAN) + +#define HAL_timer_get_count(timer_num) timer_get_count(TIMER_DEV(timer_num)) + +// TODO change this + +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void tempTC_Handler() +#endif +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void stepTC_Handler() +#endif + +extern "C" { + void tempTC_Handler(); + void stepTC_Handler(); +} + +// ------------------------ +// Public Variables +// ------------------------ + +//static HardwareTimer StepperTimer(MF_TIMER_STEP); +//static HardwareTimer TempTimer(MF_TIMER_TEMP); + +// ------------------------ +// Public functions +// ------------------------ + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +/** + * NOTE: By default libmaple sets ARPE = 1, which means the Auto reload register is preloaded (will only update with an update event) + * Thus we have to pause the timer, update the value, refresh, resume the timer. + * That seems like a big waste of time and may be better to change the timer config to ARPE = 0, so ARR can be updated any time. + * We are using a Channel in each timer in Capture/Compare mode. We could also instead use the Time Update Event Interrupt, but need to disable ARPE + * so we can change the ARR value on the fly (without calling refresh), and not get an interrupt right there because we caused an UEV. + * This mode pretty much makes 2 timers unusable for PWM since they have their counts updated all the time on ISRs. + * The way Marlin manages timer interrupts doesn't make for an efficient usage in STM32F1 + * Todo: Look at that possibility later. + */ + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + switch (timer_num) { + case MF_TIMER_STEP: + // NOTE: WE have set ARPE = 0, which means the Auto reload register is not preloaded + // and there is no need to use any compare, as in the timer mode used, setting ARR to the compare value + // will result in exactly the same effect, ie triggering an interrupt, and on top, set counter to 0 + timer_set_reload(STEP_TIMER_DEV, compare); // We reload direct ARR as needed during counting up + break; + case MF_TIMER_TEMP: + timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, compare); + break; + } +} + +FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: + // No counter to clear + timer_generate_update(STEP_TIMER_DEV); + return; + case MF_TIMER_TEMP: + timer_set_count(TEMP_TIMER_DEV, 0); + timer_generate_update(TEMP_TIMER_DEV); + return; + } +} + +#define HAL_timer_isr_epilogue(T) NOOP + +// No command is available in framework to turn off ARPE bit, which is turned on by default in libmaple. +// Needed here to reset ARPE=0 for stepper timer +FORCE_INLINE static void timer_no_ARR_preload_ARPE(timer_dev *dev) { + bb_peri_set_bit(&(dev->regs).gen->CR1, TIMER_CR1_ARPE_BIT, 0); +} + +void HAL_timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority); + +#define TIMER_OC_NO_PRELOAD 0 // Need to disable preload also on compare registers. diff --git a/src/HAL/TEENSY31_32/HAL.cpp b/src/HAL/TEENSY31_32/HAL.cpp new file mode 100644 index 0000000..2892368 --- /dev/null +++ b/src/HAL/TEENSY31_32/HAL.cpp @@ -0,0 +1,129 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for Teensy 3.2 (MK20DX256) + */ + +#ifdef __MK20DX256__ + +#include "HAL.h" +#include "../shared/Delay.h" + +#include + +// ------------------------ +// Serial ports +// ------------------------ + +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#else + #error "SERIAL_PORT must be from 0 to 3." +#endif +USBSerialType USBSerial(false, SerialUSB); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } + +uint8_t MarlinHAL::get_reset_source() { + switch (RCM_SRS0) { + case 128: return RST_POWER_ON; break; + case 64: return RST_EXTERNAL; break; + case 32: return RST_WATCHDOG; break; + // case 8: return RST_LOSS_OF_LOCK; break; + // case 4: return RST_LOSS_OF_CLOCK; break; + // case 2: return RST_LOW_VOLTAGE; break; + } + return 0; +} + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout + + void MarlinHAL::watchdog_init() { + WDOG_TOVALH = 0; + WDOG_TOVALL = WDT_TIMEOUT_MS; + WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN; + } + + void MarlinHAL::watchdog_refresh() { + // Watchdog refresh sequence + WDOG_REFRESH = 0xA602; + WDOG_REFRESH = 0xB480; + } + +#endif + +// ------------------------ +// ADC +// ------------------------ + +void MarlinHAL::adc_init() { + analog_init(); + while (ADC0_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish + NVIC_ENABLE_IRQ(IRQ_FTM1); +} + +void MarlinHAL::adc_start(const pin_t pin) { + static const uint8_t pin2sc1a[] = { + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 0, 19, 3, 31, // 0-13, we treat them as A0-A13 + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 (A0-A9) + 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, // 24-33 + 0+64, 19+64, 3+64, 31+64, // 34-37 (A10-A13) + 26, 22, 23, 27, 29, 30 // 38-43: temp. sensor, VREF_OUT, A14, bandgap, VREFH, VREFL. A14 isn't connected to anything in Teensy 3.0. + }; + ADC0_SC1A = pin2sc1a[pin]; +} + +uint16_t MarlinHAL::adc_value() { return ADC0_RA; } + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + int freeMemory() { + int free_memory; + if ((int)__brkval == 0) + free_memory = ((int)&free_memory) - ((int)&__bss_end); + else + free_memory = ((int)&free_memory) - ((int)__brkval); + return free_memory; + } +} + +#endif // __MK20DX256__ diff --git a/src/HAL/TEENSY31_32/HAL.h b/src/HAL/TEENSY31_32/HAL.h new file mode 100644 index 0000000..a7aa9f0 --- /dev/null +++ b/src/HAL/TEENSY31_32/HAL.h @@ -0,0 +1,190 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Teensy 3.2 (MK20DX256) + */ + +#define CPU_32_BIT + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" + +#include "fastio.h" + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define IS_32BIT_TEENSY 1 +#define IS_TEENSY_31_32 1 +#ifndef IS_TEENSY31 + #define IS_TEENSY32 1 +#endif + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +// ------------------------ +// Serial ports +// ------------------------ + +#include "../../core/serial_hook.h" + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; +extern USBSerialType USBSerial; + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if SERIAL_PORT == -1 + #define MYSERIAL1 USBSerial +#elif WITHIN(SERIAL_PORT, 0, 3) + DECLARE_SERIAL(SERIAL_PORT); + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "The required SERIAL_PORT must be from 0 to 3, or -1 for Native USB." +#endif + +// ------------------------ +// Types +// ------------------------ + +class libServo; +typedef libServo hal_servo_t; + +typedef int8_t pin_t; + +// ------------------------ +// Interrupts +// ------------------------ + +uint32_t __get_PRIMASK(void); // CMSIS +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() + +// ------------------------ +// ADC +// ------------------------ + +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif + +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +extern "C" int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t ch) {} + + // Begin ADC sampling on the given channel. Called from Temperature::isr! + static void adc_start(const pin_t ch); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/src/HAL/TEENSY31_32/HAL_SPI.cpp b/src/HAL/TEENSY31_32/HAL_SPI.cpp new file mode 100644 index 0000000..415c692 --- /dev/null +++ b/src/HAL/TEENSY31_32/HAL_SPI.cpp @@ -0,0 +1,130 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __MK20DX256__ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include +#include +#include "spi_pins.h" + +static SPISettings spiConfig; + +/** + * Standard SPI functions + */ + +// Initialize SPI bus +void spiBegin() { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + SET_OUTPUT(SD_SCK_PIN); + SET_INPUT(SD_MISO_PIN); + SET_OUTPUT(SD_MOSI_PIN); + + #if 0 && DISABLED(SOFTWARE_SPI) + // set SS high - may be chip select for another SPI device + #if SET_SPI_SS_HIGH + WRITE(SD_SS_PIN, HIGH); + #endif + // set a default rate + spiInit(SPI_HALF_SPEED); // 1 + #endif +} + +// Configure SPI for specified SPI speed +void spiInit(uint8_t spiRate) { + // Use data rates Marlin uses + uint32_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = 10000000; break; + case SPI_HALF_SPEED: clock = 5000000; break; + case SPI_QUARTER_SPEED: clock = 2500000; break; + case SPI_EIGHTH_SPEED: clock = 1250000; break; + case SPI_SPEED_5: clock = 625000; break; + case SPI_SPEED_6: clock = 312500; break; + default: clock = 4000000; // Default from the SPI library + } + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + SPI.begin(); +} + +// SPI receive a byte +uint8_t spiRec() { + SPI.beginTransaction(spiConfig); + const uint8_t returnByte = SPI.transfer(0xFF); + SPI.endTransaction(); + return returnByte; + //SPDR = 0xFF; + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //return SPDR; +} + +// SPI read data +void spiRead(uint8_t *buf, uint16_t nbyte) { + SPI.beginTransaction(spiConfig); + SPI.transfer(buf, nbyte); + SPI.endTransaction(); + //if (nbyte-- == 0) return; + // SPDR = 0xFF; + //for (uint16_t i = 0; i < nbyte; i++) { + // while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + // buf[i] = SPDR; + // SPDR = 0xFF; + //} + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //buf[nbyte] = SPDR; +} + +// SPI send a byte +void spiSend(uint8_t b) { + SPI.beginTransaction(spiConfig); + SPI.transfer(b); + SPI.endTransaction(); + //SPDR = b; + //while (!TEST(SPSR, SPIF)) { /* nada */ } +} + +// SPI send block +void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI.beginTransaction(spiConfig); + SPDR = token; + for (uint16_t i = 0; i < 512; i += 2) { + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i]; + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i + 1]; + } + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPI.endTransaction(); +} + + +// Begin SPI transaction, set clock, bit order, data mode +void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + spiConfig = SPISettings(spiClock, bitOrder, dataMode); + SPI.beginTransaction(spiConfig); +} + +#endif // __MK20DX256__ diff --git a/src/HAL/TEENSY31_32/MarlinSPI.h b/src/HAL/TEENSY31_32/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/TEENSY31_32/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/TEENSY31_32/Servo.cpp b/src/HAL/TEENSY31_32/Servo.cpp new file mode 100644 index 0000000..19d57cf --- /dev/null +++ b/src/HAL/TEENSY31_32/Servo.cpp @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __MK20DX256__ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "Servo.h" + +uint8_t servoPin[MAX_SERVOS] = { 0 }; + +int8_t libServo::attach(const int inPin) { + if (servoIndex >= MAX_SERVOS) return -1; + if (inPin > 0) servoPin[servoIndex] = inPin; + return super::attach(servoPin[servoIndex]); +} + +int8_t libServo::attach(const int inPin, const int inMin, const int inMax) { + if (inPin > 0) servoPin[servoIndex] = inPin; + return super::attach(servoPin[servoIndex], inMin, inMax); +} + +void libServo::move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + if (attach(0) >= 0) { + write(value); + safe_delay(servo_delay[servoIndex]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +#endif // HAS_SERVOS +#endif // __MK20DX256__ diff --git a/src/HAL/TEENSY31_32/Servo.h b/src/HAL/TEENSY31_32/Servo.h new file mode 100644 index 0000000..82b601d --- /dev/null +++ b/src/HAL/TEENSY31_32/Servo.h @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +// Inherit and expand on the official library +class libServo : public Servo { + public: + int8_t attach(const int pin); + int8_t attach(const int pin, const int min, const int max); + void move(const int value); + private: + typedef Servo super; + uint16_t min_ticks; + uint16_t max_ticks; + uint8_t servoIndex; // index into the channel data for this servo +}; diff --git a/src/HAL/TEENSY31_32/eeprom.cpp b/src/HAL/TEENSY31_32/eeprom.cpp new file mode 100644 index 0000000..d1ff940 --- /dev/null +++ b/src/HAL/TEENSY31_32/eeprom.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#ifdef __MK20DX256__ + +/** + * HAL PersistentStore for Teensy 3.2 (MK20DX256) + */ + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +#include "../shared/eeprom_api.h" +#include + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // __MK20DX256__ diff --git a/src/HAL/TEENSY31_32/endstop_interrupts.h b/src/HAL/TEENSY31_32/endstop_interrupts.h new file mode 100644 index 0000000..9c7e210 --- /dev/null +++ b/src/HAL/TEENSY31_32/endstop_interrupts.h @@ -0,0 +1,73 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Endstop Interrupts + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +/** + * Endstop interrupts for Due based targets. + * On Due, all pins support external interrupt capability. + */ + +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/TEENSY31_32/fastio.h b/src/HAL/TEENSY31_32/fastio.h new file mode 100644 index 0000000..622799e --- /dev/null +++ b/src/HAL/TEENSY31_32/fastio.h @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for Teensy 3.5 and Teensy 3.6 + * Use direct port manipulation to save scads of processor time. + * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al. + */ + +#ifndef MASK + #define MASK(PIN) _BV(PIN) +#endif + +#define GPIO_BITBAND_ADDR(reg, bit) (((uint32_t)&(reg) - 0x40000000) * 32 + (bit) * 4 + 0x42000000) +#define GPIO_BITBAND(reg, bit) (*(uint32_t *)GPIO_BITBAND_ADDR((reg), (bit))) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(PIN); WRITE(PIN, HIGH); WRITE(PIN, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/gcc-4.8.5/cpp/Stringification.html + */ + +#define _READ(P) bool(CORE_PIN ## P ## _PINREG & CORE_PIN ## P ## _BITMASK) + +#define _WRITE(P,V) do{ \ + if (V) CORE_PIN ## P ## _PORTSET = CORE_PIN ## P ## _BITMASK; \ + else CORE_PIN ## P ## _PORTCLEAR = CORE_PIN ## P ## _BITMASK; \ +}while(0) + +#define _TOGGLE(P) (*(&(CORE_PIN ## P ## _PORTCLEAR)+1) = CORE_PIN ## P ## _BITMASK) + +#define _SET_INPUT(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1); \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \ +}while(0) + +#define _SET_OUTPUT(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1)|PORT_PCR_SRE|PORT_PCR_DSE; \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 1; \ +}while(0) + +#define _SET_INPUT_PULLUP(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1) | PORT_PCR_PE | PORT_PCR_PS; \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \ +}while(0) + +#define _IS_INPUT(P) ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0) +#define _IS_OUTPUT(P) ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0) + +#define READ(IO) _READ(IO) + +#define WRITE(IO,V) _WRITE(IO,V) +#define TOGGLE(IO) _TOGGLE(IO) + +#define SET_INPUT(IO) _SET_INPUT(IO) +#define SET_INPUT_PULLUP(IO) _SET_INPUT_PULLUP(IO) +#define SET_INPUT_PULLDOWN SET_INPUT +#define SET_OUTPUT(IO) _SET_OUTPUT(IO) +#define SET_PWM SET_OUTPUT + +#define IS_INPUT(IO) _IS_INPUT(IO) +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +#define PWM_PIN(P) digitalPinHasPWM(P) + +/** + * Ports, functions, and pins + */ + +#define DIO0_PIN 8 diff --git a/src/HAL/TEENSY31_32/inc/Conditionals_LCD.h b/src/HAL/TEENSY31_32/inc/Conditionals_LCD.h new file mode 100644 index 0000000..54ec166 --- /dev/null +++ b/src/HAL/TEENSY31_32/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/TEENSY31_32." +#endif diff --git a/src/HAL/TEENSY31_32/inc/Conditionals_adv.h b/src/HAL/TEENSY31_32/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/TEENSY31_32/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/TEENSY31_32/inc/Conditionals_post.h b/src/HAL/TEENSY31_32/inc/Conditionals_post.h new file mode 100644 index 0000000..998f1dc --- /dev/null +++ b/src/HAL/TEENSY31_32/inc/Conditionals_post.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define USE_WIRED_EEPROM 1 +#endif diff --git a/src/HAL/TEENSY31_32/inc/SanityCheck.h b/src/HAL/TEENSY31_32/inc/SanityCheck.h new file mode 100644 index 0000000..dbce187 --- /dev/null +++ b/src/HAL/TEENSY31_32/inc/SanityCheck.h @@ -0,0 +1,46 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test TEENSY35_36 specific configuration values for errors at compile-time. + */ + +#if ENABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is not yet implemented for Teensy 3.1/3.2. Disable EMERGENCY_PARSER to continue." +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on Teensy 3.1/3.2." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on Teensy 3.1/3.2." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on Teensy 3.1/3.2." +#endif + +#if USING_PULLDOWNS + #error "PULLDOWN pin mode is not available on Teensy 3.1/3.2 boards." +#endif diff --git a/src/HAL/TEENSY31_32/pinsDebug.h b/src/HAL/TEENSY31_32/pinsDebug.h new file mode 100644 index 0000000..d4a91ce --- /dev/null +++ b/src/HAL/TEENSY31_32/pinsDebug.h @@ -0,0 +1 @@ +#error "PINS_DEBUGGING is not yet supported for Teensy 3.1 / 3.2!" diff --git a/src/HAL/TEENSY31_32/spi_pins.h b/src/HAL/TEENSY31_32/spi_pins.h new file mode 100644 index 0000000..6d0d05f --- /dev/null +++ b/src/HAL/TEENSY31_32/spi_pins.h @@ -0,0 +1,27 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define SD_SCK_PIN 13 +#define SD_MISO_PIN 12 +#define SD_MOSI_PIN 11 +#define SD_SS_PIN 20 // SDSS // A.28, A.29, B.21, C.26, C.29 diff --git a/src/HAL/TEENSY31_32/timers.cpp b/src/HAL/TEENSY31_32/timers.cpp new file mode 100644 index 0000000..f217715 --- /dev/null +++ b/src/HAL/TEENSY31_32/timers.cpp @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Timers for Teensy 3.2 (MK20DX256) + */ + +#ifdef __MK20DX256__ + +#include "../../inc/MarlinConfig.h" + +/** \brief Instruction Synchronization Barrier + Instruction Synchronization Barrier flushes the pipeline in the processor, + so that all instructions following the ISB are fetched from cache or + memory, after the instruction has been completed. +*/ +FORCE_INLINE static void __ISB() { + __asm__ __volatile__("isb 0xF":::"memory"); +} + +/** \brief Data Synchronization Barrier + This function acts as a special kind of Data Memory Barrier. + It completes when all explicit memory accesses before this instruction complete. +*/ +FORCE_INLINE static void __DSB() { + __asm__ __volatile__("dsb 0xF":::"memory"); +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + switch (timer_num) { + case MF_TIMER_STEP: + FTM0_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; + FTM0_SC = 0x00; // Set this to zero before changing the modulus + FTM0_CNT = 0x0000; // Reset the count to zero + FTM0_MOD = 0xFFFF; // max modulus = 65535 + FTM0_C0V = (FTM0_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value + FTM0_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM0_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 8 + FTM0_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; + break; + case MF_TIMER_TEMP: + FTM1_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; // Disable write protection, Enable FTM1 + FTM1_SC = 0x00; // Set this to zero before changing the modulus + FTM1_CNT = 0x0000; // Reset the count to zero + FTM1_MOD = 0xFFFF; // max modulus = 65535 + FTM1_C0V = (FTM1_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value 65535 + FTM1_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM1_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 4 + FTM1_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; + break; + } +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_FTM1); break; + } +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_FTM1); break; + } + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return NVIC_IS_ENABLED(IRQ_FTM0); + case MF_TIMER_TEMP: return NVIC_IS_ENABLED(IRQ_FTM1); + } + return false; +} + +void HAL_timer_isr_prologue(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: + FTM0_CNT = 0x0000; + FTM0_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag + FTM0_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag + break; + case MF_TIMER_TEMP: + FTM1_CNT = 0x0000; + FTM1_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag + FTM1_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag + break; + } +} + +#endif // __MK20DX256__ diff --git a/src/HAL/TEENSY31_32/timers.h b/src/HAL/TEENSY31_32/timers.h new file mode 100644 index 0000000..9fcbb6f --- /dev/null +++ b/src/HAL/TEENSY31_32/timers.h @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Timers for Teensy 3.2 (MK20DX256) + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define FTM0_TIMER_PRESCALE 8 +#define FTM1_TIMER_PRESCALE 4 +#define FTM0_TIMER_PRESCALE_BITS 0b011 +#define FTM1_TIMER_PRESCALE_BITS 0b010 + +#define FTM0_TIMER_RATE (F_BUS / (FTM0_TIMER_PRESCALE)) // 60MHz / 8 = 7500kHz +#define FTM1_TIMER_RATE (F_BUS / (FTM1_TIMER_PRESCALE)) // 60MHz / 4 = 15MHz + +#define HAL_TIMER_RATE (FTM0_TIMER_RATE) + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif + +#define TEMP_TIMER_FREQUENCY 1000 + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void ftm0_isr() //void TC3_Handler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void ftm1_isr() //void TC4_Handler() +#endif + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + switch (timer_num) { + case MF_TIMER_STEP: FTM0_C0V = compare; break; + case MF_TIMER_TEMP: FTM1_C0V = compare; break; + } +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return FTM0_C0V; + case MF_TIMER_TEMP: return FTM1_C0V; + } + return 0; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return FTM0_CNT; + case MF_TIMER_TEMP: return FTM1_CNT; + } + return 0; +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +void HAL_timer_isr_prologue(const uint8_t timer_num); +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/TEENSY35_36/HAL.cpp b/src/HAL/TEENSY35_36/HAL.cpp new file mode 100644 index 0000000..bc02ac1 --- /dev/null +++ b/src/HAL/TEENSY35_36/HAL.cpp @@ -0,0 +1,156 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +#include "HAL.h" +#include "../shared/Delay.h" + +#include + +// ------------------------ +// Serial ports +// ------------------------ + +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#endif + +USBSerialType USBSerial(false, SerialUSB); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } + +uint8_t MarlinHAL::get_reset_source() { + switch (RCM_SRS0) { + case 128: return RST_POWER_ON; break; + case 64: return RST_EXTERNAL; break; + case 32: return RST_WATCHDOG; break; + // case 8: return RST_LOSS_OF_LOCK; break; + // case 4: return RST_LOSS_OF_CLOCK; break; + // case 2: return RST_LOW_VOLTAGE; break; + } + return 0; +} + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout + + void MarlinHAL::watchdog_init() { + WDOG_TOVALH = 0; + WDOG_TOVALL = WDT_TIMEOUT_MS; + WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN; + } + + void MarlinHAL::watchdog_refresh() { + // Watchdog refresh sequence + WDOG_REFRESH = 0xA602; + WDOG_REFRESH = 0xB480; + } + +#endif + +// ------------------------ +// ADC +// ------------------------ + +int8_t MarlinHAL::adc_select; + +void MarlinHAL::adc_init() { + analog_init(); + while (ADC0_SC3 & ADC_SC3_CAL) { /* Wait for calibration to finish */ } + while (ADC1_SC3 & ADC_SC3_CAL) { /* Wait for calibration to finish */ } + NVIC_ENABLE_IRQ(IRQ_FTM1); +} + +void MarlinHAL::adc_start(const pin_t adc_pin) { + static const uint8_t pin2sc1a[] = { + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 3, 19+128, 14+128, 15+128, // 0-13 -> A0-A13 + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 are A0-A9 + 255, 255, 255, 255, 255, 255, 255, // 24-30 are digital only + 14+128, 15+128, 17, 18, 4+128, 5+128, 6+128, 7+128, 17+128, // 31-39 are A12-A20 + 255, 255, 255, 255, 255, 255, 255, 255, 255, // 40-48 are digital only + 10+128, 11+128, // 49-50 are A23-A24 + 255, 255, 255, 255, 255, 255, 255, // 51-57 are digital only + 255, 255, 255, 255, 255, 255, // 58-63 (sd card pins) are digital only + 3, 19+128, // 64-65 are A10-A11 + 23, 23+128,// 66-67 are A21-A22 (DAC pins) + 1, 1+128, // 68-69 are A25-A26 (unused USB host port on Teensy 3.5) + 26, // 70 is Temperature Sensor + 18+128 // 71 is Vref + }; + const uint16_t pin = pin2sc1a[adc_pin]; + if (pin == 0xFF) { + adc_select = -1; // Digital only + } + else if (pin & 0x80) { + adc_select = 1; + ADC1_SC1A = pin & 0x7F; + } + else { + adc_select = 0; + ADC0_SC1A = pin; + } +} + +uint16_t MarlinHAL::adc_value() { + switch (adc_select) { + case 0: return ADC0_RA; + case 1: return ADC1_RA; + } + return 0; +} + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + int freeMemory() { + int free_memory; + if ((int)__brkval == 0) + free_memory = ((int)&free_memory) - ((int)&__bss_end); + else + free_memory = ((int)&free_memory) - ((int)__brkval); + return free_memory; + } +} + +#endif // __MK64FX512__ || __MK66FX1M0__ diff --git a/src/HAL/TEENSY35_36/HAL.h b/src/HAL/TEENSY35_36/HAL.h new file mode 100644 index 0000000..2a192e4 --- /dev/null +++ b/src/HAL/TEENSY35_36/HAL.h @@ -0,0 +1,197 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#define CPU_32_BIT + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" + +#include "fastio.h" + +#include +#include + +// ------------------------ +// Defines +// ------------------------ + +#define IS_32BIT_TEENSY 1 +#define IS_TEENSY_35_36 1 +#ifdef __MK66FX1M0__ + #define IS_TEENSY36 1 +#else // __MK64FX512__ + #define IS_TEENSY35 1 +#endif + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +#undef sq +#define sq(x) ((x)*(x)) + +// ------------------------ +// Serial ports +// ------------------------ + +#include "../../core/serial_hook.h" + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; +extern USBSerialType USBSerial; + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if SERIAL_PORT == -1 + #define MYSERIAL1 USBSerial +#elif WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) + DECLARE_SERIAL(SERIAL_PORT); +#else + #error "SERIAL_PORT must be from 0 to 3, or -1 for Native USB." +#endif + +// ------------------------ +// Types +// ------------------------ + +class libServo; +typedef libServo hal_servo_t; + +typedef int8_t pin_t; + +// ------------------------ +// Interrupts +// ------------------------ + +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() + +// ------------------------ +// ADC +// ------------------------ + +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif + +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +extern "C" int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return true; } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static int8_t adc_select; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t) {} + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/src/HAL/TEENSY35_36/HAL_SPI.cpp b/src/HAL/TEENSY35_36/HAL_SPI.cpp new file mode 100644 index 0000000..d80f57b --- /dev/null +++ b/src/HAL/TEENSY35_36/HAL_SPI.cpp @@ -0,0 +1,125 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL SPI for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include +#include +#include "spi_pins.h" + +static SPISettings spiConfig; + +void spiBegin() { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + SET_OUTPUT(SD_SCK_PIN); + SET_INPUT(SD_MISO_PIN); + SET_OUTPUT(SD_MOSI_PIN); + + #if 0 && DISABLED(SOFTWARE_SPI) + // set SS high - may be chip select for another SPI device + #if SET_SPI_SS_HIGH + WRITE(SD_SS_PIN, HIGH); + #endif + // set a default rate + spiInit(SPI_HALF_SPEED); // 1 + #endif +} + +void spiInit(uint8_t spiRate) { + // Use Marlin data-rates + uint32_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = 10000000; break; + case SPI_HALF_SPEED: clock = 5000000; break; + case SPI_QUARTER_SPEED: clock = 2500000; break; + case SPI_EIGHTH_SPEED: clock = 1250000; break; + case SPI_SPEED_5: clock = 625000; break; + case SPI_SPEED_6: clock = 312500; break; + default: + clock = 4000000; // Default from the SPI library + } + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + SPI.begin(); +} + +uint8_t spiRec() { + SPI.beginTransaction(spiConfig); + uint8_t returnByte = SPI.transfer(0xFF); + SPI.endTransaction(); + return returnByte; + //SPDR = 0xFF; + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //return SPDR; +} + +void spiRead(uint8_t *buf, uint16_t nbyte) { + SPI.beginTransaction(spiConfig); + SPI.transfer(buf, nbyte); + SPI.endTransaction(); + //if (nbyte-- == 0) return; + // SPDR = 0xFF; + //for (uint16_t i = 0; i < nbyte; i++) { + // while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + // buf[i] = SPDR; + // SPDR = 0xFF; + //} + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //buf[nbyte] = SPDR; +} + +void spiSend(uint8_t b) { + SPI.beginTransaction(spiConfig); + SPI.transfer(b); + SPI.endTransaction(); + //SPDR = b; + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } +} + +void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI.beginTransaction(spiConfig); + SPDR = token; + for (uint16_t i = 0; i < 512; i += 2) { + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i]; + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i + 1]; + } + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPI.endTransaction(); +} + +// Begin SPI transaction, set clock, bit order, data mode +void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + spiConfig = SPISettings(spiClock, bitOrder, dataMode); + SPI.beginTransaction(spiConfig); +} + +#endif // __MK64FX512__ || __MK66FX1M0__ diff --git a/src/HAL/TEENSY35_36/MarlinSPI.h b/src/HAL/TEENSY35_36/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/TEENSY35_36/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/TEENSY35_36/Servo.cpp b/src/HAL/TEENSY35_36/Servo.cpp new file mode 100644 index 0000000..0338585 --- /dev/null +++ b/src/HAL/TEENSY35_36/Servo.cpp @@ -0,0 +1,59 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Servo for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "Servo.h" + +uint8_t servoPin[MAX_SERVOS] = { 0 }; + +int8_t libServo::attach(const int inPin) { + if (servoIndex >= MAX_SERVOS) return -1; + if (inPin > 0) servoPin[servoIndex] = inPin; + return super::attach(servoPin[servoIndex]); +} + +int8_t libServo::attach(const int inPin, const int inMin, const int inMax) { + if (inPin > 0) servoPin[servoIndex] = inPin; + return super::attach(servoPin[servoIndex], inMin, inMax); +} + +void libServo::move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + if (attach(0) >= 0) { + write(value); + safe_delay(servo_delay[servoIndex]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +#endif // HAS_SERVOS +#endif // __MK64FX512__ || __MK66FX1M0__ diff --git a/src/HAL/TEENSY35_36/Servo.h b/src/HAL/TEENSY35_36/Servo.h new file mode 100644 index 0000000..719011f --- /dev/null +++ b/src/HAL/TEENSY35_36/Servo.h @@ -0,0 +1,41 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Servo for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#include + +// Inherit and expand on core Servo library +class libServo : public Servo { + public: + int8_t attach(const int pin); + int8_t attach(const int pin, const int min, const int max); + void move(const int value); + private: + typedef Servo super; + uint16_t min_ticks; + uint16_t max_ticks; + uint8_t servoIndex; // Index into the channel data for this servo +}; diff --git a/src/HAL/TEENSY35_36/eeprom.cpp b/src/HAL/TEENSY35_36/eeprom.cpp new file mode 100644 index 0000000..b80e93b --- /dev/null +++ b/src/HAL/TEENSY35_36/eeprom.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +/** + * HAL PersistentStore for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +#include "../shared/eeprom_api.h" +#include + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // __MK64FX512__ || __MK66FX1M0__ diff --git a/src/HAL/TEENSY35_36/endstop_interrupts.h b/src/HAL/TEENSY35_36/endstop_interrupts.h new file mode 100644 index 0000000..a300248 --- /dev/null +++ b/src/HAL/TEENSY35_36/endstop_interrupts.h @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Endstop Interrupts for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +/** + * Endstop interrupts for Due based targets. + * On Due, all pins support external interrupt capability. + */ +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/TEENSY35_36/fastio.h b/src/HAL/TEENSY35_36/fastio.h new file mode 100644 index 0000000..622799e --- /dev/null +++ b/src/HAL/TEENSY35_36/fastio.h @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for Teensy 3.5 and Teensy 3.6 + * Use direct port manipulation to save scads of processor time. + * Contributed by Triffid_Hunter and modified by Kliment, thinkyhead, Bob-the-Kuhn, et.al. + */ + +#ifndef MASK + #define MASK(PIN) _BV(PIN) +#endif + +#define GPIO_BITBAND_ADDR(reg, bit) (((uint32_t)&(reg) - 0x40000000) * 32 + (bit) * 4 + 0x42000000) +#define GPIO_BITBAND(reg, bit) (*(uint32_t *)GPIO_BITBAND_ADDR((reg), (bit))) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(PIN); WRITE(PIN, HIGH); WRITE(PIN, LOW); + * + * Why double up on these macros? see https://gcc.gnu.org/onlinedocs/gcc-4.8.5/cpp/Stringification.html + */ + +#define _READ(P) bool(CORE_PIN ## P ## _PINREG & CORE_PIN ## P ## _BITMASK) + +#define _WRITE(P,V) do{ \ + if (V) CORE_PIN ## P ## _PORTSET = CORE_PIN ## P ## _BITMASK; \ + else CORE_PIN ## P ## _PORTCLEAR = CORE_PIN ## P ## _BITMASK; \ +}while(0) + +#define _TOGGLE(P) (*(&(CORE_PIN ## P ## _PORTCLEAR)+1) = CORE_PIN ## P ## _BITMASK) + +#define _SET_INPUT(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1); \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \ +}while(0) + +#define _SET_OUTPUT(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1)|PORT_PCR_SRE|PORT_PCR_DSE; \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 1; \ +}while(0) + +#define _SET_INPUT_PULLUP(P) do{ \ + CORE_PIN ## P ## _CONFIG = PORT_PCR_MUX(1) | PORT_PCR_PE | PORT_PCR_PS; \ + GPIO_BITBAND(CORE_PIN ## P ## _DDRREG , CORE_PIN ## P ## _BIT) = 0; \ +}while(0) + +#define _IS_INPUT(P) ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0) +#define _IS_OUTPUT(P) ((CORE_PIN ## P ## _DDRREG & CORE_PIN ## P ## _BITMASK) == 0) + +#define READ(IO) _READ(IO) + +#define WRITE(IO,V) _WRITE(IO,V) +#define TOGGLE(IO) _TOGGLE(IO) + +#define SET_INPUT(IO) _SET_INPUT(IO) +#define SET_INPUT_PULLUP(IO) _SET_INPUT_PULLUP(IO) +#define SET_INPUT_PULLDOWN SET_INPUT +#define SET_OUTPUT(IO) _SET_OUTPUT(IO) +#define SET_PWM SET_OUTPUT + +#define IS_INPUT(IO) _IS_INPUT(IO) +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) + +#define PWM_PIN(P) digitalPinHasPWM(P) + +/** + * Ports, functions, and pins + */ + +#define DIO0_PIN 8 diff --git a/src/HAL/TEENSY35_36/inc/Conditionals_LCD.h b/src/HAL/TEENSY35_36/inc/Conditionals_LCD.h new file mode 100644 index 0000000..632ee53 --- /dev/null +++ b/src/HAL/TEENSY35_36/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/TEENSY35_36." +#endif diff --git a/src/HAL/TEENSY35_36/inc/Conditionals_adv.h b/src/HAL/TEENSY35_36/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/TEENSY35_36/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/TEENSY35_36/inc/Conditionals_post.h b/src/HAL/TEENSY35_36/inc/Conditionals_post.h new file mode 100644 index 0000000..998f1dc --- /dev/null +++ b/src/HAL/TEENSY35_36/inc/Conditionals_post.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define USE_WIRED_EEPROM 1 +#endif diff --git a/src/HAL/TEENSY35_36/inc/SanityCheck.h b/src/HAL/TEENSY35_36/inc/SanityCheck.h new file mode 100644 index 0000000..3308707 --- /dev/null +++ b/src/HAL/TEENSY35_36/inc/SanityCheck.h @@ -0,0 +1,46 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test TEENSY35_36 specific configuration values for errors at compile-time. + */ + +#if ENABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is not yet implemented for Teensy 3.5/3.6. Disable EMERGENCY_PARSER to continue." +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on Teensy 3.5/3.6." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on Teensy 3.5/3.6." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on Teensy 3.5/3.6." +#endif + +#if USING_PULLDOWNS + #error "PULLDOWN pin mode is not available on Teensy 3.5/3.6 boards." +#endif diff --git a/src/HAL/TEENSY35_36/pinsDebug.h b/src/HAL/TEENSY35_36/pinsDebug.h new file mode 100644 index 0000000..7a2e1d6 --- /dev/null +++ b/src/HAL/TEENSY35_36/pinsDebug.h @@ -0,0 +1,111 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Pins Debugging for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS +#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin + +#define FTM0_CH0_PIN 22 +#define FTM0_CH1_PIN 23 +#define FTM0_CH2_PIN 9 +#define FTM0_CH3_PIN 10 +#define FTM0_CH4_PIN 6 +#define FTM0_CH5_PIN 20 +#define FTM0_CH6_PIN 21 +#define FTM0_CH7_PIN 5 +#define FTM1_CH0_PIN 3 +#define FTM1_CH1_PIN 4 +#define FTM2_CH0_PIN 29 +#define FTM2_CH1_PIN 30 +#define FTM3_CH0_PIN 2 +#define FTM3_CH1_PIN 14 +#define FTM3_CH2_PIN 7 +#define FTM3_CH3_PIN 8 +#define FTM3_CH4_PIN 35 +#define FTM3_CH5_PIN 36 +#define FTM3_CH6_PIN 37 +#define FTM3_CH7_PIN 38 +#ifdef __MK66FX1M0__ // Teensy3.6 + #define TPM1_CH0_PIN 16 + #define TPM1_CH1_PIN 17 +#endif + +#define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(0) && (P) <= analogInputToDigitalPin(9)) || ((P) >= analogInputToDigitalPin(12) && (P) <= analogInputToDigitalPin(20)) + +void HAL_print_analog_pin(char buffer[], int8_t pin) { + if (pin <= 23) sprintf_P(buffer, PSTR("(A%2d) "), int(pin - 14)); + else if (pin <= 39) sprintf_P(buffer, PSTR("(A%2d) "), int(pin - 19)); +} + +void HAL_analog_pin_state(char buffer[], int8_t pin) { + if (pin <= 23) sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 14)); + else if (pin <= 39) sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 19)); +} + +#define PWM_PRINT(V) do{ sprintf_P(buffer, PSTR("PWM: %4d"), 22); SERIAL_ECHO(buffer); }while(0) +#define FTM_CASE(N,Z) \ + case FTM##N##_CH##Z##_PIN: \ + if (FTM##N##_C##Z##V) { \ + PWM_PRINT(FTM##N##_C##Z##V); \ + return true; \ + } else return false + +/** + * Print a pin's PWM status. + * Return true if it's currently a PWM pin. + */ +bool HAL_pwm_status(int8_t pin) { + char buffer[20]; // for the sprintf statements + switch (pin) { + FTM_CASE(0,0); + FTM_CASE(0,1); + FTM_CASE(0,2); + FTM_CASE(0,3); + FTM_CASE(0,4); + FTM_CASE(0,5); + FTM_CASE(0,6); + FTM_CASE(0,7); + FTM_CASE(1,0); + FTM_CASE(1,1); + FTM_CASE(2,0); + FTM_CASE(2,1); + FTM_CASE(3,0); + FTM_CASE(3,1); + FTM_CASE(3,2); + FTM_CASE(3,3); + FTM_CASE(3,4); + FTM_CASE(3,5); + FTM_CASE(3,6); + FTM_CASE(3,7); + + case NOT_ON_TIMER: + default: + return false; + } + SERIAL_ECHOPGM(" "); +} + +static void HAL_pwm_details(uint8_t pin) { /* TODO */ } diff --git a/src/HAL/TEENSY35_36/spi_pins.h b/src/HAL/TEENSY35_36/spi_pins.h new file mode 100644 index 0000000..cfffdc9 --- /dev/null +++ b/src/HAL/TEENSY35_36/spi_pins.h @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL SPI Pins for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#define SD_SCK_PIN 13 +#define SD_MISO_PIN 12 +#define SD_MOSI_PIN 11 +#define SD_SS_PIN 20 // SDSS // A.28, A.29, B.21, C.26, C.29 diff --git a/src/HAL/TEENSY35_36/timers.cpp b/src/HAL/TEENSY35_36/timers.cpp new file mode 100644 index 0000000..39095fb --- /dev/null +++ b/src/HAL/TEENSY35_36/timers.cpp @@ -0,0 +1,113 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Timers for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#if defined(__MK64FX512__) || defined(__MK66FX1M0__) + +#include "../../inc/MarlinConfig.h" + +/** \brief Instruction Synchronization Barrier + Instruction Synchronization Barrier flushes the pipeline in the processor, + so that all instructions following the ISB are fetched from cache or + memory, after the instruction has been completed. +*/ +FORCE_INLINE static void __ISB() { + __asm__ __volatile__("isb 0xF":::"memory"); +} + +/** \brief Data Synchronization Barrier + This function acts as a special kind of Data Memory Barrier. + It completes when all explicit memory accesses before this instruction complete. +*/ +FORCE_INLINE static void __DSB() { + __asm__ __volatile__("dsb 0xF":::"memory"); +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + switch (timer_num) { + case MF_TIMER_STEP: + FTM0_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; + FTM0_SC = 0x00; // Set this to zero before changing the modulus + FTM0_CNT = 0x0000; // Reset the count to zero + FTM0_MOD = 0xFFFF; // max modulus = 65535 + FTM0_C0V = (FTM0_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value + FTM0_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM0_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 8 + FTM0_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; + break; + case MF_TIMER_TEMP: + FTM1_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; // Disable write protection, Enable FTM1 + FTM1_SC = 0x00; // Set this to zero before changing the modulus + FTM1_CNT = 0x0000; // Reset the count to zero + FTM1_MOD = 0xFFFF; // max modulus = 65535 + FTM1_C0V = (FTM1_TIMER_RATE) / frequency; // Initial FTM Channel 0 compare value 65535 + FTM1_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM1_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 4 + FTM1_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; + break; + } +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_FTM1); break; + } +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_FTM1); break; + } + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + __DSB(); + __ISB(); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return NVIC_IS_ENABLED(IRQ_FTM0); + case MF_TIMER_TEMP: return NVIC_IS_ENABLED(IRQ_FTM1); + } + return false; +} + +void HAL_timer_isr_prologue(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: + FTM0_CNT = 0x0000; + FTM0_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag + FTM0_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag + break; + case MF_TIMER_TEMP: + FTM1_CNT = 0x0000; + FTM1_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag + FTM1_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag + break; + } +} + +#endif // Teensy3.5 or Teensy3.6 diff --git a/src/HAL/TEENSY35_36/timers.h b/src/HAL/TEENSY35_36/timers.h new file mode 100644 index 0000000..8af79d7 --- /dev/null +++ b/src/HAL/TEENSY35_36/timers.h @@ -0,0 +1,112 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Timers for Teensy 3.5 (MK64FX512) and Teensy 3.6 (MK66FX1M0) + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF + +#define FTM0_TIMER_PRESCALE 8 +#define FTM1_TIMER_PRESCALE 4 +#define FTM0_TIMER_PRESCALE_BITS 0b011 +#define FTM1_TIMER_PRESCALE_BITS 0b010 + +#define FTM0_TIMER_RATE (F_BUS / FTM0_TIMER_PRESCALE) // 60MHz / 8 = 7500kHz +#define FTM1_TIMER_RATE (F_BUS / FTM1_TIMER_PRESCALE) // 60MHz / 4 = 15MHz + +#define HAL_TIMER_RATE (FTM0_TIMER_RATE) + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif + +#define TEMP_TIMER_FREQUENCY 1000 + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void ftm0_isr() //void TC3_Handler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void ftm1_isr() //void TC4_Handler() +#endif + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + switch (timer_num) { + case MF_TIMER_STEP: FTM0_C0V = compare; break; + case MF_TIMER_TEMP: FTM1_C0V = compare; break; + } +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return FTM0_C0V; + case MF_TIMER_TEMP: return FTM1_C0V; + } + return 0; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return FTM0_CNT; + case MF_TIMER_TEMP: return FTM1_CNT; + } + return 0; +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +void HAL_timer_isr_prologue(const uint8_t timer_num); +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/TEENSY40_41/HAL.cpp b/src/HAL/TEENSY40_41/HAL.cpp new file mode 100644 index 0000000..1d02ab8 --- /dev/null +++ b/src/HAL/TEENSY40_41/HAL.cpp @@ -0,0 +1,210 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL for Teensy 4.0 / 4.1 (IMXRT1062) + */ + +#ifdef __IMXRT1062__ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include "../shared/Delay.h" +#include "timers.h" +#include + +// ------------------------ +// Serial ports +// ------------------------ + +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#endif +USBSerialType USBSerial(false, SerialUSB); + +// ------------------------ +// FastIO +// ------------------------ + +bool is_output(pin_t pin) { + const struct digital_pin_bitband_and_config_table_struct *p; + p = digital_pin_to_info_PGM + pin; + return (*(p->reg + 1) & p->mask); +} + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } + +uint8_t MarlinHAL::get_reset_source() { + switch (SRC_SRSR & 0xFF) { + case 1: return RST_POWER_ON; break; + case 2: return RST_SOFTWARE; break; + case 4: return RST_EXTERNAL; break; + //case 8: return RST_BROWN_OUT; break; + case 16: return RST_WATCHDOG; break; + case 64: return RST_JTAG; break; + //case 128: return RST_OVERTEMP; break; + } + return 0; +} + +void MarlinHAL::clear_reset_source() { + uint32_t reset_source = SRC_SRSR; + SRC_SRSR = reset_source; +} + +// ------------------------ +// Watchdog Timer +// ------------------------ + +#if ENABLED(USE_WATCHDOG) + + #define WDT_TIMEOUT TERN(WATCHDOG_DURATION_8S, 8, 4) // 4 or 8 second timeout + + constexpr uint8_t timeoutval = (WDT_TIMEOUT - 0.5f) / 0.5f; + + void MarlinHAL::watchdog_init() { + CCM_CCGR3 |= CCM_CCGR3_WDOG1(3); // enable WDOG1 clocks + WDOG1_WMCR = 0; // disable power down PDE + WDOG1_WCR |= WDOG_WCR_SRS | WDOG_WCR_WT(timeoutval); + WDOG1_WCR |= WDOG_WCR_WDE | WDOG_WCR_WDT | WDOG_WCR_SRE; + } + + void MarlinHAL::watchdog_refresh() { + // Watchdog refresh sequence + WDOG1_WSR = 0x5555; + WDOG1_WSR = 0xAAAA; + } + +#endif + +// ------------------------ +// ADC +// ------------------------ + +int8_t MarlinHAL::adc_select; + +void MarlinHAL::adc_init() { + analog_init(); + while (ADC1_GC & ADC_GC_CAL) { /* wait */ } + while (ADC2_GC & ADC_GC_CAL) { /* wait */ } +} + +void MarlinHAL::adc_start(const pin_t adc_pin) { + static const uint8_t pin2sc1a[] = { + 0x07, // 0/A0 AD_B1_02 + 0x08, // 1/A1 AD_B1_03 + 0x0C, // 2/A2 AD_B1_07 + 0x0B, // 3/A3 AD_B1_06 + 0x06, // 4/A4 AD_B1_01 + 0x05, // 5/A5 AD_B1_00 + 0x0F, // 6/A6 AD_B1_10 + 0x00, // 7/A7 AD_B1_11 + 0x0D, // 8/A8 AD_B1_08 + 0x0E, // 9/A9 AD_B1_09 + 0x01, // 24/A10 AD_B0_12 + 0x02, // 25/A11 AD_B0_13 + 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 + 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 + 0x07, // 14/A0 AD_B1_02 + 0x08, // 15/A1 AD_B1_03 + 0x0C, // 16/A2 AD_B1_07 + 0x0B, // 17/A3 AD_B1_06 + 0x06, // 18/A4 AD_B1_01 + 0x05, // 19/A5 AD_B1_00 + 0x0F, // 20/A6 AD_B1_10 + 0x00, // 21/A7 AD_B1_11 + 0x0D, // 22/A8 AD_B1_08 + 0x0E, // 23/A9 AD_B1_09 + 0x01, // 24/A10 AD_B0_12 + 0x02, // 25/A11 AD_B0_13 + 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 + 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 + #ifdef ARDUINO_TEENSY41 + 0xFF, // 28 + 0xFF, // 29 + 0xFF, // 30 + 0xFF, // 31 + 0xFF, // 32 + 0xFF, // 33 + 0xFF, // 34 + 0xFF, // 35 + 0xFF, // 36 + 0xFF, // 37 + 0x81, // 38/A14 AD_B1_12 - only on ADC2, 1 + 0x82, // 39/A15 AD_B1_13 - only on ADC2, 2 + 0x09, // 40/A16 AD_B1_04 + 0x0A, // 41/A17 AD_B1_05 + #endif + }; + const uint16_t pin = pin2sc1a[adc_pin]; + if (pin == 0xFF) { + adc_select = -1; // Digital only + } + else if (pin & 0x80) { + adc_select = 1; + ADC2_HC0 = pin & 0x7F; + } + else { + adc_select = 0; + ADC1_HC0 = pin; + } +} + +uint16_t MarlinHAL::adc_value() { + switch (adc_select) { + case 0: + while (!(ADC1_HS & ADC_HS_COCO0)) { /* wait */ } + return ADC1_R0; + case 1: + while (!(ADC2_HS & ADC_HS_COCO0)) { /* wait */ } + return ADC2_R0; + } + return 0; +} + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#define __bss_end _ebss + +extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + // Doesn't work on Teensy 4.x + uint32_t freeMemory() { + uint32_t free_memory; + free_memory = ((uint32_t)&free_memory) - (((uint32_t)__brkval) ?: ((uint32_t)&__bss_end)); + return free_memory; + } +} + +#endif // __IMXRT1062__ diff --git a/src/HAL/TEENSY40_41/HAL.h b/src/HAL/TEENSY40_41/HAL.h new file mode 100644 index 0000000..c54a2e8 --- /dev/null +++ b/src/HAL/TEENSY40_41/HAL.h @@ -0,0 +1,219 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#define CPU_32_BIT + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" + +#include "fastio.h" + +#include +#include + +#if HAS_ETHERNET + #include "../../feature/ethernet.h" +#endif + +// ------------------------ +// Defines +// ------------------------ + +#define IS_32BIT_TEENSY 1 +#define IS_TEENSY_40_41 1 +#ifndef IS_TEENSY40 + #define IS_TEENSY41 1 +#endif + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +#undef sq +#define sq(x) ((x)*(x)) + +// Don't place string constants in PROGMEM +#undef PSTR +#define PSTR(str) ({static const char *data = (str); &data[0];}) + +// ------------------------ +// Serial ports +// ------------------------ + +#include "../../core/serial_hook.h" + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; +extern USBSerialType USBSerial; + +#define _MSERIAL(X) MSerial##X +#define MSERIAL(X) _MSERIAL(X) + +#if SERIAL_PORT == -1 + #define MYSERIAL1 SerialUSB +#elif WITHIN(SERIAL_PORT, 0, 8) + DECLARE_SERIAL(SERIAL_PORT); + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "The required SERIAL_PORT must be from 0 to 8, or -1 for Native USB." +#endif + +#ifdef SERIAL_PORT_2 + #if SERIAL_PORT_2 == -1 + #define MYSERIAL2 usbSerial + #elif SERIAL_PORT_2 == -2 + #define MYSERIAL2 ethernet.telnetClient + #elif WITHIN(SERIAL_PORT_2, 0, 8) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 8, or -1 for Native USB, or -2 for Ethernet." + #endif +#endif + +// ------------------------ +// Types +// ------------------------ + +class libServo; +typedef libServo hal_servo_t; + +typedef int8_t pin_t; + +// ------------------------ +// Interrupts +// ------------------------ + +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() + +// ------------------------ +// ADC +// ------------------------ + +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif + +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 +#define HAL_ADC_FILTERED // turn off ADC oversampling + +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + +// FastIO +bool is_output(pin_t pin); + +// ------------------------ +// Free Memory Accessor +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +extern "C" uint32_t freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + // Watchdog + static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {}); + static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {}); + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_primask(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static int8_t adc_select; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) {} + + // Begin ADC sampling on the given pin. Called from Temperature::isr! + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/src/HAL/TEENSY40_41/HAL_SPI.cpp b/src/HAL/TEENSY40_41/HAL_SPI.cpp new file mode 100644 index 0000000..9dcb812 --- /dev/null +++ b/src/HAL/TEENSY40_41/HAL_SPI.cpp @@ -0,0 +1,141 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL SPI for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#ifdef __IMXRT1062__ + +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + +#include +#include +#include "spi_pins.h" + +static SPISettings spiConfig; + +// ------------------------ +// Public functions +// ------------------------ + +#if ENABLED(SOFTWARE_SPI) + // ------------------------ + // Software SPI + // ------------------------ + #error "Software SPI not supported for Teensy 4. Use Hardware SPI." +#else + +// ------------------------ +// Hardware SPI +// ------------------------ + +void spiBegin() { + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + //SET_OUTPUT(SD_SCK_PIN); + //SET_INPUT(SD_MISO_PIN); + //SET_OUTPUT(SD_MOSI_PIN); + + #if 0 && DISABLED(SOFTWARE_SPI) + // set SS high - may be chip select for another SPI device + #if SET_SPI_SS_HIGH + WRITE(SD_SS_PIN, HIGH); + #endif + // set a default rate + spiInit(SPI_HALF_SPEED); // 1 + #endif +} + +void spiInit(uint8_t spiRate) { + // Use Marlin data-rates + uint32_t clock; + switch (spiRate) { + case SPI_FULL_SPEED: clock = 10000000; break; + case SPI_HALF_SPEED: clock = 5000000; break; + case SPI_QUARTER_SPEED: clock = 2500000; break; + case SPI_EIGHTH_SPEED: clock = 1250000; break; + case SPI_SPEED_5: clock = 625000; break; + case SPI_SPEED_6: clock = 312500; break; + default: + clock = 4000000; // Default from the SPI library + } + spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); + SPI.begin(); +} + +uint8_t spiRec() { + SPI.beginTransaction(spiConfig); + uint8_t returnByte = SPI.transfer(0xFF); + SPI.endTransaction(); + return returnByte; + //SPDR = 0xFF; + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //return SPDR; +} + +void spiRead(uint8_t *buf, uint16_t nbyte) { + SPI.beginTransaction(spiConfig); + SPI.transfer(buf, nbyte); + SPI.endTransaction(); + //if (nbyte-- == 0) return; + // SPDR = 0xFF; + //for (uint16_t i = 0; i < nbyte; i++) { + // while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + // buf[i] = SPDR; + // SPDR = 0xFF; + //} + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } + //buf[nbyte] = SPDR; +} + +void spiSend(uint8_t b) { + SPI.beginTransaction(spiConfig); + SPI.transfer(b); + SPI.endTransaction(); + //SPDR = b; + //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } +} + +void spiSendBlock(uint8_t token, const uint8_t *buf) { + SPI.beginTransaction(spiConfig); + SPDR = token; + for (uint16_t i = 0; i < 512; i += 2) { + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i]; + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPDR = buf[i + 1]; + } + while (!TEST(SPSR, SPIF)) { /* nada */ }; + SPI.endTransaction(); +} + +// Begin SPI transaction, set clock, bit order, data mode +void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode) { + spiConfig = SPISettings(spiClock, bitOrder, dataMode); + SPI.beginTransaction(spiConfig); +} + +#endif // SOFTWARE_SPI +#endif // __IMXRT1062__ diff --git a/src/HAL/TEENSY40_41/MarlinSPI.h b/src/HAL/TEENSY40_41/MarlinSPI.h new file mode 100644 index 0000000..0c447ba --- /dev/null +++ b/src/HAL/TEENSY40_41/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/src/HAL/TEENSY40_41/Servo.cpp b/src/HAL/TEENSY40_41/Servo.cpp new file mode 100644 index 0000000..ffb1102 --- /dev/null +++ b/src/HAL/TEENSY40_41/Servo.cpp @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Servo for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#ifdef __IMXRT1062__ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "Servo.h" + +int8_t libServo::attach(const int inPin) { + if (inPin > 0) servoPin = inPin; + return super::attach(servoPin); +} + +int8_t libServo::attach(const int inPin, const int inMin, const int inMax) { + if (inPin > 0) servoPin = inPin; + return super::attach(servoPin, inMin, inMax); +} + +void libServo::move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + if (attach(0) >= 0) { + write(value); + safe_delay(servo_delay[servoIndex]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +void libServo::detach() { + // PWMServo library does not have detach() function + //super::detach(); +} + +#endif // HAS_SERVOS +#endif // __IMXRT1062__ diff --git a/src/HAL/TEENSY40_41/Servo.h b/src/HAL/TEENSY40_41/Servo.h new file mode 100644 index 0000000..699fd70 --- /dev/null +++ b/src/HAL/TEENSY40_41/Servo.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Servo for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#include + +// Inherit and expand on core Servo library +class libServo : public PWMServo { + public: + int8_t attach(const int pin); + int8_t attach(const int pin, const int min, const int max); + void move(const int value); + void detach(void); + private: + typedef PWMServo super; + uint8_t servoPin; + uint16_t min_ticks; + uint16_t max_ticks; + uint8_t servoIndex; // Index into the channel data for this servo +}; diff --git a/src/HAL/TEENSY40_41/eeprom.cpp b/src/HAL/TEENSY40_41/eeprom.cpp new file mode 100644 index 0000000..3cd376e --- /dev/null +++ b/src/HAL/TEENSY40_41/eeprom.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#ifdef __IMXRT1062__ + +/** + * HAL PersistentStore for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + +#include "../shared/eeprom_api.h" +#include + +#ifndef MARLIN_EEPROM_SIZE + #define MARLIN_EEPROM_SIZE size_t(E2END + 1) +#endif +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t * const p = (uint8_t * const)pos; + uint8_t v = *value; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t c = eeprom_read_byte((uint8_t*)pos); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // USE_WIRED_EEPROM +#endif // __IMXRT1062__ diff --git a/src/HAL/TEENSY40_41/endstop_interrupts.h b/src/HAL/TEENSY40_41/endstop_interrupts.h new file mode 100644 index 0000000..4c3ddec --- /dev/null +++ b/src/HAL/TEENSY40_41/endstop_interrupts.h @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Endstop Interrupts for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + * + * Without endstop interrupts the endstop pins must be polled continually in + * the temperature-ISR via endstops.update(), most of the time finding no change. + * With this feature endstops.update() is called only when we know that at + * least one endstop has changed state, saving valuable CPU cycles. + * + * This feature only works when all used endstop pins can generate an 'external interrupt'. + * + * Test whether pins issue interrupts on your board by flashing 'pin_interrupt_test.ino'. + * (Located in Marlin/buildroot/share/pin_interrupt_test/pin_interrupt_test.ino) + */ + +#include "../../module/endstops.h" + +// One ISR for all EXT-Interrupts +void endstop_ISR() { endstops.update(); } + +/** + * Endstop interrupts for Due based targets. + * On Due, all pins support external interrupt capability. + */ +void setup_endstop_interrupts() { + #define _ATTACH(P) attachInterrupt(digitalPinToInterrupt(P), endstop_ISR, CHANGE) + TERN_(HAS_X_MAX, _ATTACH(X_MAX_PIN)); + TERN_(HAS_X_MIN, _ATTACH(X_MIN_PIN)); + TERN_(HAS_Y_MAX, _ATTACH(Y_MAX_PIN)); + TERN_(HAS_Y_MIN, _ATTACH(Y_MIN_PIN)); + TERN_(HAS_Z_MAX, _ATTACH(Z_MAX_PIN)); + TERN_(HAS_Z_MIN, _ATTACH(Z_MIN_PIN)); + TERN_(HAS_X2_MAX, _ATTACH(X2_MAX_PIN)); + TERN_(HAS_X2_MIN, _ATTACH(X2_MIN_PIN)); + TERN_(HAS_Y2_MAX, _ATTACH(Y2_MAX_PIN)); + TERN_(HAS_Y2_MIN, _ATTACH(Y2_MIN_PIN)); + TERN_(HAS_Z2_MAX, _ATTACH(Z2_MAX_PIN)); + TERN_(HAS_Z2_MIN, _ATTACH(Z2_MIN_PIN)); + TERN_(HAS_Z3_MAX, _ATTACH(Z3_MAX_PIN)); + TERN_(HAS_Z3_MIN, _ATTACH(Z3_MIN_PIN)); + TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); + TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); + TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); +} diff --git a/src/HAL/TEENSY40_41/fastio.h b/src/HAL/TEENSY40_41/fastio.h new file mode 100644 index 0000000..52f991d --- /dev/null +++ b/src/HAL/TEENSY40_41/fastio.h @@ -0,0 +1,58 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2017 Victor Perez + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O interfaces for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + * These use GPIO functions instead of Direct Port Manipulation, as on AVR. + */ + +#ifndef PWM + #define PWM OUTPUT +#endif + +#define READ(IO) digitalRead(IO) +#define WRITE(IO,V) digitalWrite(IO,V) + +#define _GET_MODE(IO) !is_output(IO) +#define _SET_MODE(IO,M) pinMode(IO, M) +#define _SET_OUTPUT(IO) pinMode(IO, OUTPUT) /*!< Output Push Pull Mode & GPIO_NOPULL */ + +#define OUT_WRITE(IO,V) do{ _SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +#define SET_INPUT(IO) _SET_MODE(IO, INPUT) /*!< Input Floating Mode */ +#define SET_INPUT_PULLUP(IO) _SET_MODE(IO, INPUT_PULLUP) /*!< Input with Pull-up activation */ +#define SET_INPUT_PULLDOWN(IO) _SET_MODE(IO, INPUT_PULLDOWN) /*!< Input with Pull-down activation */ +#define SET_OUTPUT(IO) OUT_WRITE(IO, LOW) +#define SET_PWM(IO) _SET_MODE(IO, PWM) + +#define TOGGLE(IO) OUT_WRITE(IO, !READ(IO)) + +#define IS_INPUT(IO) !is_output(IO) +#define IS_OUTPUT(IO) is_output(IO) + +#define PWM_PIN(P) digitalPinHasPWM(P) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h b/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h new file mode 100644 index 0000000..6a85409 --- /dev/null +++ b/src/HAL/TEENSY40_41/inc/Conditionals_LCD.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if HAS_SPI_TFT || HAS_FSMC_TFT + #error "Sorry! TFT displays are not available for HAL/TEENSY40_41." +#endif diff --git a/src/HAL/TEENSY40_41/inc/Conditionals_adv.h b/src/HAL/TEENSY40_41/inc/Conditionals_adv.h new file mode 100644 index 0000000..5f1c4b1 --- /dev/null +++ b/src/HAL/TEENSY40_41/inc/Conditionals_adv.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once diff --git a/src/HAL/TEENSY40_41/inc/Conditionals_post.h b/src/HAL/TEENSY40_41/inc/Conditionals_post.h new file mode 100644 index 0000000..998f1dc --- /dev/null +++ b/src/HAL/TEENSY40_41/inc/Conditionals_post.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#if USE_FALLBACK_EEPROM + #define USE_WIRED_EEPROM 1 +#endif diff --git a/src/HAL/TEENSY40_41/inc/SanityCheck.h b/src/HAL/TEENSY40_41/inc/SanityCheck.h new file mode 100644 index 0000000..3d2668d --- /dev/null +++ b/src/HAL/TEENSY40_41/inc/SanityCheck.h @@ -0,0 +1,42 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * Test TEENSY41 specific configuration values for errors at compile-time. + */ + +#if ENABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is not yet implemented for Teensy 4.0/4.1. Disable EMERGENCY_PARSER to continue." +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on Teensy 4.0/4.1." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on Teensy 4.0/4.1." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on Teensy 4.0/4.1." +#endif diff --git a/src/HAL/TEENSY40_41/pinsDebug.h b/src/HAL/TEENSY40_41/pinsDebug.h new file mode 100644 index 0000000..94b85ea --- /dev/null +++ b/src/HAL/TEENSY40_41/pinsDebug.h @@ -0,0 +1,154 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Pins Debugging for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#warning "PINS_DEBUGGING is not fully supported for Teensy 4.0 / 4.1 so 'M43' may cause hangs." + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS + +#define digitalRead_mod(p) extDigitalRead(p) // AVR digitalRead disabled PWM before it read the pin +#define PRINT_PORT(p) +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%02d"), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital +#define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL ? 1 : 0) +#define DIGITAL_PIN_TO_ANALOG_PIN(p) int(p - analogInputToDigitalPin(0)) +#define IS_ANALOG(P) ((P) >= analogInputToDigitalPin(0) && (P) <= analogInputToDigitalPin(13)) || ((P) >= analogInputToDigitalPin(14) && (P) <= analogInputToDigitalPin(17)) +#define pwm_status(pin) HAL_pwm_status(pin) +#define GET_PINMODE(PIN) (VALID_PIN(pin) && IS_OUTPUT(pin)) +#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin + +struct pwm_pin_info_struct { + uint8_t type; // 0=no pwm, 1=flexpwm, 2=quad + uint8_t module; // 0-3, 0-3 + uint8_t channel; // 0=X, 1=A, 2=B + uint8_t muxval; // +}; + +#define M(a, b) ((((a) - 1) << 4) | (b)) + +const struct pwm_pin_info_struct pwm_pin_info[] = { + {1, M(1, 1), 0, 4}, // FlexPWM1_1_X 0 // AD_B0_03 + {1, M(1, 0), 0, 4}, // FlexPWM1_0_X 1 // AD_B0_02 + {1, M(4, 2), 1, 1}, // FlexPWM4_2_A 2 // EMC_04 + {1, M(4, 2), 2, 1}, // FlexPWM4_2_B 3 // EMC_05 + {1, M(2, 0), 1, 1}, // FlexPWM2_0_A 4 // EMC_06 + {1, M(2, 1), 1, 1}, // FlexPWM2_1_A 5 // EMC_08 + {1, M(2, 2), 1, 2}, // FlexPWM2_2_A 6 // B0_10 + {1, M(1, 3), 2, 6}, // FlexPWM1_3_B 7 // B1_01 + {1, M(1, 3), 1, 6}, // FlexPWM1_3_A 8 // B1_00 + {1, M(2, 2), 2, 2}, // FlexPWM2_2_B 9 // B0_11 + {2, M(1, 0), 0, 1}, // QuadTimer1_0 10 // B0_00 + {2, M(1, 2), 0, 1}, // QuadTimer1_2 11 // B0_02 + {2, M(1, 1), 0, 1}, // QuadTimer1_1 12 // B0_01 + {2, M(2, 0), 0, 1}, // QuadTimer2_0 13 // B0_03 + {2, M(3, 2), 0, 1}, // QuadTimer3_2 14 // AD_B1_02 + {2, M(3, 3), 0, 1}, // QuadTimer3_3 15 // AD_B1_03 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {2, M(3, 1), 0, 1}, // QuadTimer3_1 18 // AD_B1_01 + {2, M(3, 0), 0, 1}, // QuadTimer3_0 19 // AD_B1_00 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(4, 0), 1, 1}, // FlexPWM4_0_A 22 // AD_B1_08 + {1, M(4, 1), 1, 1}, // FlexPWM4_1_A 23 // AD_B1_09 + {1, M(1, 2), 0, 4}, // FlexPWM1_2_X 24 // AD_B0_12 + {1, M(1, 3), 0, 4}, // FlexPWM1_3_X 25 // AD_B0_13 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(3, 1), 2, 1}, // FlexPWM3_1_B 28 // EMC_32 + {1, M(3, 1), 1, 1}, // FlexPWM3_1_A 29 // EMC_31 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(2, 0), 2, 1}, // FlexPWM2_0_B 33 // EMC_07 + #ifdef ARDUINO_TEENSY40 + {1, M(1, 1), 2, 1}, // FlexPWM1_1_B 34 // SD_B0_03 + {1, M(1, 1), 1, 1}, // FlexPWM1_1_A 35 // SD_B0_02 + {1, M(1, 0), 2, 1}, // FlexPWM1_0_B 36 // SD_B0_01 + {1, M(1, 0), 1, 1}, // FlexPWM1_0_A 37 // SD_B0_00 + {1, M(1, 2), 2, 1}, // FlexPWM1_2_B 38 // SD_B0_05 + {1, M(1, 2), 1, 1}, // FlexPWM1_2_A 39 // SD_B0_04 + #endif + #ifdef ARDUINO_TEENSY41 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(2, 3), 1, 6}, // FlexPWM2_3_A 36 // B1_00 + {1, M(2, 3), 2, 6}, // FlexPWM2_3_B 37 // B1_01 + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {0, M(1, 0), 0, 0}, + {1, M(1, 1), 2, 1}, // FlexPWM1_1_B 42 // SD_B0_03 + {1, M(1, 1), 1, 1}, // FlexPWM1_1_A 43 // SD_B0_02 + {1, M(1, 0), 2, 1}, // FlexPWM1_0_B 44 // SD_B0_01 + {1, M(1, 0), 1, 1}, // FlexPWM1_0_A 45 // SD_B0_00 + {1, M(1, 2), 2, 1}, // FlexPWM1_2_B 46 // SD_B0_05 + {1, M(1, 2), 1, 1}, // FlexPWM1_2_A 47 // SD_B0_04 + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_0_B + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_2_A + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_2_B + {1, M(3, 3), 2, 1}, // FlexPWM3_3_B 51 // EMC_22 + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_1_B + {0, M(1, 0), 0, 0}, // duplicate FlexPWM1_1_A + {1, M(3, 0), 1, 1}, // FlexPWM3_0_A 53 // EMC_29 + #endif +}; + +void HAL_print_analog_pin(char buffer[], int8_t pin) { + if (pin <= 23) sprintf_P(buffer, PSTR("(A%2d) "), int(pin - 14)); + else if (pin <= 41) sprintf_P(buffer, PSTR("(A%2d) "), int(pin - 24)); +} + +void HAL_analog_pin_state(char buffer[], int8_t pin) { + if (pin <= 23) sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 14)); + else if (pin <= 41) sprintf_P(buffer, PSTR("Analog in =% 5d"), analogRead(pin - 24)); +} + +#define PWM_PRINT(V) do{ sprintf_P(buffer, PSTR("PWM: %4d"), V); SERIAL_ECHO(buffer); }while(0) + +/** + * Print a pin's PWM status. + * Return true if it's currently a PWM pin. + */ +bool HAL_pwm_status(int8_t pin) { + char buffer[20]; // for the sprintf statements + const struct pwm_pin_info_struct *info; + + if (pin >= CORE_NUM_DIGITAL) return 0; + info = pwm_pin_info + pin; + + if (info->type == 0) return 0; + + /* TODO decode pwm value from timers */ + // for now just indicate if output is set as pwm + PWM_PRINT(*(portConfigRegister(pin)) == info->muxval); + return (*(portConfigRegister(pin)) == info->muxval); +} + +static void pwm_details(uint8_t pin) { /* TODO */ } diff --git a/src/HAL/TEENSY40_41/spi_pins.h b/src/HAL/TEENSY40_41/spi_pins.h new file mode 100644 index 0000000..ba4a2c7 --- /dev/null +++ b/src/HAL/TEENSY40_41/spi_pins.h @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL SPI Pins for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#define SD_SCK_PIN 13 +#define SD_MISO_PIN 12 +#define SD_MOSI_PIN 11 +#define SD_SS_PIN 20 // SDSS // A.28, A.29, B.21, C.26, C.29 diff --git a/src/HAL/TEENSY40_41/timers.cpp b/src/HAL/TEENSY40_41/timers.cpp new file mode 100644 index 0000000..ed99f65 --- /dev/null +++ b/src/HAL/TEENSY40_41/timers.cpp @@ -0,0 +1,106 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL Timers for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#ifdef __IMXRT1062__ + +#include "../../inc/MarlinConfig.h" + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + switch (timer_num) { + case MF_TIMER_STEP: + CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode + CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON); + + GPT1_CR = 0; // disable timer + GPT1_SR = 0x3F; // clear all prior status + GPT1_PR = GPT1_TIMER_PRESCALE - 1; + GPT1_CR |= GPT_CR_CLKSRC(1); //clock selection #1 (peripheral clock = 150 MHz) + GPT1_CR |= GPT_CR_ENMOD; //reset count to zero before enabling + GPT1_CR |= GPT_CR_OM1(1); // toggle mode + GPT1_OCR1 = (GPT1_TIMER_RATE / frequency) -1; // Initial compare value + GPT1_IR = GPT_IR_OF1IE; // Compare3 value + GPT1_CR |= GPT_CR_EN; //enable GPT2 counting at 150 MHz + + OUT_WRITE(15, HIGH); + attachInterruptVector(IRQ_GPT1, &stepTC_Handler); + NVIC_SET_PRIORITY(IRQ_GPT1, 16); + break; + case MF_TIMER_TEMP: + CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode + CCM_CCGR0 |= CCM_CCGR0_GPT2_BUS(CCM_CCGR_ON); + + GPT2_CR = 0; // disable timer + GPT2_SR = 0x3F; // clear all prior status + GPT2_PR = GPT2_TIMER_PRESCALE - 1; + GPT2_CR |= GPT_CR_CLKSRC(1); //clock selection #1 (peripheral clock = 150 MHz) + GPT2_CR |= GPT_CR_ENMOD; //reset count to zero before enabling + GPT2_CR |= GPT_CR_OM1(1); // toggle mode + GPT2_OCR1 = (GPT2_TIMER_RATE / frequency) -1; // Initial compare value + GPT2_IR = GPT_IR_OF1IE; // Compare3 value + GPT2_CR |= GPT_CR_EN; //enable GPT2 counting at 150 MHz + + OUT_WRITE(14, HIGH); + attachInterruptVector(IRQ_GPT2, &tempTC_Handler); + NVIC_SET_PRIORITY(IRQ_GPT2, 32); + break; + } +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_GPT1); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_GPT2); break; + } +} + +void HAL_timer_disable_interrupt(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_GPT1); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_GPT2); break; + } + + // We NEED memory barriers to ensure Interrupts are actually disabled! + // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) + asm volatile("dsb"); +} + +bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return (NVIC_IS_ENABLED(IRQ_GPT1)); + case MF_TIMER_TEMP: return (NVIC_IS_ENABLED(IRQ_GPT2)); + } + return false; +} + +void HAL_timer_isr_prologue(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: GPT1_SR = GPT_IR_OF1IE; break; // clear OF3 bit + case MF_TIMER_TEMP: GPT2_SR = GPT_IR_OF1IE; break; // clear OF3 bit + } + asm volatile("dsb"); +} + +#endif // __IMXRT1062__ diff --git a/src/HAL/TEENSY40_41/timers.h b/src/HAL/TEENSY40_41/timers.h new file mode 100644 index 0000000..77fe095 --- /dev/null +++ b/src/HAL/TEENSY40_41/timers.h @@ -0,0 +1,117 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL Timers for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint32_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFE + +#define GPT_TIMER_RATE F_BUS_ACTUAL // 150MHz + +#define GPT1_TIMER_PRESCALE 2 +#define GPT2_TIMER_PRESCALE 10 + +#define GPT1_TIMER_RATE (GPT_TIMER_RATE / GPT1_TIMER_PRESCALE) // 75MHz +#define GPT2_TIMER_RATE (GPT_TIMER_RATE / GPT2_TIMER_PRESCALE) // 15MHz + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif + +#define TEMP_TIMER_RATE 1000000 +#define TEMP_TIMER_FREQUENCY 1000 + +#define STEPPER_TIMER_RATE GPT1_TIMER_RATE +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) +#define STEPPER_TIMER_PRESCALE ((GPT_TIMER_RATE / 1000000) / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void stepTC_Handler() // GPT1_Handler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void tempTC_Handler() // GPT2_Handler() +#endif + +extern "C" { + void stepTC_Handler(); + void tempTC_Handler(); +} + +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { + switch (timer_num) { + case MF_TIMER_STEP: GPT1_OCR1 = compare - 1; break; + case MF_TIMER_TEMP: GPT2_OCR1 = compare - 1; break; + } +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return GPT1_OCR1; + case MF_TIMER_TEMP: return GPT2_OCR1; + } + return 0; +} + +FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { + switch (timer_num) { + case MF_TIMER_STEP: return GPT1_CNT; + case MF_TIMER_TEMP: return GPT2_CNT; + } + return 0; +} + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +void HAL_timer_isr_prologue(const uint8_t timer_num); +//void HAL_timer_isr_epilogue(const uint8_t timer_num) {} +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/src/HAL/platforms.h b/src/HAL/platforms.h new file mode 100644 index 0000000..28fe28e --- /dev/null +++ b/src/HAL/platforms.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#define XSTR(V...) #V + +#ifdef __AVR__ + #define HAL_PATH(PATH, NAME) XSTR(PATH/AVR/NAME) +#elif defined(ARDUINO_ARCH_SAM) + #define HAL_PATH(PATH, NAME) XSTR(PATH/DUE/NAME) +#elif defined(__MK20DX256__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY31_32/NAME) +#elif defined(__MK64FX512__) || defined(__MK66FX1M0__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY35_36/NAME) +#elif defined(__IMXRT1062__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/TEENSY40_41/NAME) +#elif defined(TARGET_LPC1768) + #define HAL_PATH(PATH, NAME) XSTR(PATH/LPC1768/NAME) +#elif defined(__STM32F1__) || defined(TARGET_STM32F1) + #define HAL_PATH(PATH, NAME) XSTR(PATH/STM32F1/NAME) +#elif defined(ARDUINO_ARCH_STM32) + #ifndef HAL_STM32 + #define HAL_STM32 + #endif + #define HAL_PATH(PATH, NAME) XSTR(PATH/STM32/NAME) +#elif defined(ARDUINO_ARCH_ESP32) + #define HAL_PATH(PATH, NAME) XSTR(PATH/ESP32/NAME) +#elif defined(__PLAT_LINUX__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/LINUX/NAME) +#elif defined(__PLAT_NATIVE_SIM__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/NATIVE_SIM/NAME) +#elif defined(__SAMD51__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/SAMD51/NAME) +#else + #error "Unsupported Platform!" +#endif diff --git a/src/HAL/shared/Delay.cpp b/src/HAL/shared/Delay.cpp new file mode 100644 index 0000000..c64376d --- /dev/null +++ b/src/HAL/shared/Delay.cpp @@ -0,0 +1,178 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "Delay.h" + +#include "../../inc/MarlinConfig.h" + +#if defined(__arm__) || defined(__thumb__) + + static uint32_t ASM_CYCLES_PER_ITERATION = 4; // Initial bet which will be adjusted in calibrate_delay_loop + + // Simple assembler loop counting down + void delay_asm(uint32_t cy) { + cy = _MAX(cy / ASM_CYCLES_PER_ITERATION, 1U); // Zero is forbidden here + __asm__ __volatile__( + A(".syntax unified") // is to prevent CM0,CM1 non-unified syntax + L("1") + A("subs %[cnt],#1") + A("bne 1b") + : [cnt]"+r"(cy) // output: +r means input+output + : // input: + : "cc" // clobbers: + ); + } + + // We can't use CMSIS since it's not available on all platform, so fallback to hardcoded register values + #define HW_REG(X) *(volatile uint32_t *)(X) + #define _DWT_CTRL 0xE0001000 + #define _DWT_CYCCNT 0xE0001004 // CYCCNT is 32bits, takes 37s or so to wrap. + #define _DEM_CR 0xE000EDFC + #define _LAR 0xE0001FB0 + + // Use hardware cycle counter instead, it's much safer + void delay_dwt(uint32_t count) { + // Reuse the ASM_CYCLES_PER_ITERATION variable to avoid wasting another useless variable + uint32_t start = HW_REG(_DWT_CYCCNT) - ASM_CYCLES_PER_ITERATION, elapsed; + do { + elapsed = HW_REG(_DWT_CYCCNT) - start; + } while (elapsed < count); + } + + // Pointer to asm function, calling the functions has a ~20 cycles overhead + DelayImpl DelayCycleFnc = delay_asm; + + void calibrate_delay_loop() { + // Check if we have a working DWT implementation in the CPU (see https://developer.arm.com/documentation/ddi0439/b/Data-Watchpoint-and-Trace-Unit/DWT-Programmers-Model) + if (!HW_REG(_DWT_CTRL)) { + // No DWT present, so fallback to plain old ASM nop counting + // Unfortunately, we don't exactly know how many iteration it'll take to decrement a counter in a loop + // It depends on the CPU architecture, the code current position (flash vs SRAM) + // So, instead of wild guessing and making mistake, instead + // compute it once for all + ASM_CYCLES_PER_ITERATION = 1; + // We need to fetch some reference clock before waiting + cli(); + uint32_t start = micros(); + delay_asm(1000); // On a typical CPU running in MHz, waiting 1000 "unknown cycles" means it'll take between 1ms to 6ms, that's perfectly acceptable + uint32_t end = micros(); + sei(); + uint32_t expectedCycles = (end - start) * ((F_CPU) / 1000000UL); // Convert microseconds to cycles + // Finally compute the right scale + ASM_CYCLES_PER_ITERATION = (uint32_t)(expectedCycles / 1000); + + // No DWT present, likely a Cortex M0 so NOP counting is our best bet here + DelayCycleFnc = delay_asm; + } + else { + // Enable DWT counter + // From https://stackoverflow.com/a/41188674/1469714 + HW_REG(_DEM_CR) = HW_REG(_DEM_CR) | 0x01000000; // Enable trace + #if __CORTEX_M == 7 + HW_REG(_LAR) = 0xC5ACCE55; // Unlock access to DWT registers, see https://developer.arm.com/documentation/ihi0029/e/ section B2.3.10 + #endif + HW_REG(_DWT_CYCCNT) = 0; // Clear DWT cycle counter + HW_REG(_DWT_CTRL) = HW_REG(_DWT_CTRL) | 1; // Enable DWT cycle counter + + // Then calibrate the constant offset from the counter + ASM_CYCLES_PER_ITERATION = 0; + uint32_t s = HW_REG(_DWT_CYCCNT); + uint32_t e = HW_REG(_DWT_CYCCNT); // (e - s) contains the number of cycle required to read the cycle counter + delay_dwt(0); + uint32_t f = HW_REG(_DWT_CYCCNT); // (f - e) contains the delay to call the delay function + the time to read the cycle counter + ASM_CYCLES_PER_ITERATION = (f - e) - (e - s); + + // Use safer DWT function + DelayCycleFnc = delay_dwt; + } + } + + #if ENABLED(MARLIN_DEV_MODE) + void dump_delay_accuracy_check() { + auto report_call_time = [](FSTR_P const name, FSTR_P const unit, const uint32_t cycles, const uint32_t total, const bool do_flush=true) { + SERIAL_ECHOPGM("Calling "); + SERIAL_ECHOF(name); + SERIAL_ECHOLNPGM(" for ", cycles); + SERIAL_ECHOF(unit); + SERIAL_ECHOLNPGM(" took: ", total); + SERIAL_CHAR(' '); + SERIAL_ECHOF(unit); + if (do_flush) SERIAL_FLUSHTX(); + }; + + uint32_t s, e; + + SERIAL_ECHOLNPGM("Computed delay calibration value: ", ASM_CYCLES_PER_ITERATION); + SERIAL_FLUSH(); + // Display the results of the calibration above + constexpr uint32_t testValues[] = { 1, 5, 10, 20, 50, 100, 150, 200, 350, 500, 750, 1000 }; + for (auto i : testValues) { + s = micros(); DELAY_US(i); e = micros(); + report_call_time(F("delay"), F("us"), i, e - s); + } + + if (HW_REG(_DWT_CTRL)) { + static FSTR_P cyc = F("cycles"); + static FSTR_P dcd = F("DELAY_CYCLES directly "); + + for (auto i : testValues) { + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(i); e = HW_REG(_DWT_CYCCNT); + report_call_time(F("runtime delay"), cyc, i, e - s); + } + + // Measure the delay to call a real function compared to a function pointer + s = HW_REG(_DWT_CYCCNT); delay_dwt(1); e = HW_REG(_DWT_CYCCNT); + report_call_time(F("delay_dwt"), cyc, 1, e - s); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 1); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 1, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 5); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 5, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(10); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 10, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(20); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 20, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(50); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 50, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(100); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 100, e - s, false); + + s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(200); e = HW_REG(_DWT_CYCCNT); + report_call_time(dcd, cyc, 200, e - s, false); + } + } + #endif // MARLIN_DEV_MODE + + +#else + + void calibrate_delay_loop() {} + #if ENABLED(MARLIN_DEV_MODE) + void dump_delay_accuracy_check() { SERIAL_ECHOPGM("N/A on this platform"); } + #endif + +#endif diff --git a/src/HAL/shared/Delay.h b/src/HAL/shared/Delay.h new file mode 100644 index 0000000..a6795a7 --- /dev/null +++ b/src/HAL/shared/Delay.h @@ -0,0 +1,219 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../inc/MarlinConfigPre.h" + +/** + * Busy wait delay cycles routines: + * + * DELAY_CYCLES(count): Delay execution in cycles + * DELAY_NS(count): Delay execution in nanoseconds + * DELAY_US(count): Delay execution in microseconds + */ + +#include "../../core/macros.h" + +void calibrate_delay_loop(); + +#if defined(__arm__) || defined(__thumb__) + + // We want to have delay_cycle function with the lowest possible overhead, so we adjust at the function at runtime based on the current CPU best feature + typedef void (*DelayImpl)(uint32_t); + extern DelayImpl DelayCycleFnc; + + // I've measured 36 cycles on my system to call the cycle waiting method, but it shouldn't change much to have a bit more margin, it only consume a bit more flash + #define TRIP_POINT_FOR_CALLING_FUNCTION 40 + + // A simple recursive template class that output exactly one 'nop' of code per recursion + template struct NopWriter { + FORCE_INLINE static void build() { + __asm__ __volatile__("nop"); + NopWriter::build(); + } + }; + // End the loop + template <> struct NopWriter<0> { FORCE_INLINE static void build() {} }; + + namespace Private { + // Split recursing template in 2 different class so we don't reach the maximum template instantiation depth limit + template struct Helper { + FORCE_INLINE static void build() { + DelayCycleFnc(N - 2); // Approximative cost of calling the function (might be off by one or 2 cycles) + } + }; + + template struct Helper { + FORCE_INLINE static void build() { + NopWriter::build(); + } + }; + + template <> struct Helper { + FORCE_INLINE static void build() {} + }; + + } + // Select a behavior based on the constexpr'ness of the parameter + // If called with a compile-time parameter, then write as many NOP as required to reach the asked cycle count + // (there is some tripping point here to start looping when it's more profitable than gruntly executing NOPs) + // If not called from a compile-time parameter, fallback to a runtime loop counting version instead + template + struct SmartDelay { + FORCE_INLINE SmartDelay(int) { + if (Cycles == 0) return; + Private::Helper::build(); + } + }; + // Runtime version below. There is no way this would run under less than ~TRIP_POINT_FOR_CALLING_FUNCTION cycles + template + struct SmartDelay { + FORCE_INLINE SmartDelay(int v) { DelayCycleFnc(v); } + }; + + #define DELAY_CYCLES(X) do { SmartDelay _smrtdly_X(X); } while(0) + + #if GCC_VERSION <= 70000 + #define DELAY_CYCLES_VAR(X) DelayCycleFnc(X) + #else + #define DELAY_CYCLES_VAR DELAY_CYCLES + #endif + + // For delay in microseconds, no smart delay selection is required, directly call the delay function + // Teensy compiler is too old and does not accept smart delay compile-time / run-time selection correctly + #define DELAY_US(x) DelayCycleFnc((x) * ((F_CPU) / 1000000UL)) + +#elif defined(__AVR__) + FORCE_INLINE static void __delay_up_to_3c(uint8_t cycles) { + switch (cycles) { + case 3: + __asm__ __volatile__(A("RJMP .+0") A("NOP")); + break; + case 2: + __asm__ __volatile__(A("RJMP .+0")); + break; + case 1: + __asm__ __volatile__(A("NOP")); + break; + } + } + + // Delay in cycles + FORCE_INLINE static void DELAY_CYCLES(uint16_t cycles) { + if (__builtin_constant_p(cycles)) { + if (cycles <= 3) { + __delay_up_to_3c(cycles); + } + else if (cycles == 4) { + __delay_up_to_3c(2); + __delay_up_to_3c(2); + } + else { + cycles -= 1 + 4; // Compensate for the first LDI (1) and the first round (4) + __delay_up_to_3c(cycles % 4); + + cycles /= 4; + // The following code burns [1 + 4 * (rounds+1)] cycles + uint16_t dummy; + __asm__ __volatile__( + // "manually" load counter from constants, otherwise the compiler may optimize this part away + A("LDI %A[rounds], %[l]") // 1c + A("LDI %B[rounds], %[h]") // 1c (compensating the non branching BRCC) + L("1") + A("SBIW %[rounds], 1") // 2c + A("BRCC 1b") // 2c when branching, else 1c (end of loop) + : // Outputs ... + [rounds] "=w" (dummy) // Restrict to a wo (=) 16 bit register pair (w) + : // Inputs ... + [l] "M" (cycles%256), // Restrict to 0..255 constant (M) + [h] "M" (cycles/256) // Restrict to 0..255 constant (M) + :// Clobbers ... + "cc" // Indicate we are modifying flags like Carry (cc) + ); + } + } + else { + __asm__ __volatile__( + L("1") + A("SBIW %[cycles], 4") // 2c + A("BRCC 1b") // 2c when branching, else 1c (end of loop) + : [cycles] "+w" (cycles) // output: Restrict to a rw (+) 16 bit register pair (w) + : // input: - + : "cc" // clobbers: We are modifying flags like Carry (cc) + ); + } + } + + // Delay in microseconds + #define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL)) + + #define DELAY_CYCLES_VAR DELAY_CYCLES + +#elif defined(ESP32) || defined(__PLAT_LINUX__) || defined(__PLAT_NATIVE_SIM__) + + // DELAY_CYCLES specified inside platform + + // Delay in microseconds + #define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL)) +#else + + #error "Unsupported MCU architecture" + +#endif + +/************************************************************** + * Delay in nanoseconds. Requires the F_CPU macro. + * These macros follow avr-libc delay conventions. + * + * For AVR there are three possible operation modes, due to its + * slower clock speeds and thus coarser delay resolution. For + * example, when F_CPU = 16000000 the resolution is 62.5ns. + * + * Round up (default) + * Round up the delay according to the CPU clock resolution. + * e.g., 100 will give a delay of 2 cycles (125ns). + * + * Round down (DELAY_NS_ROUND_DOWN) + * Round down the delay according to the CPU clock resolution. + * e.g., 100 will be rounded down to 1 cycle (62.5ns). + * + * Nearest (DELAY_NS_ROUND_CLOSEST) + * Round the delay to the nearest number of clock cycles. + * e.g., 165 will be rounded up to 3 cycles (187.5ns) because + * it's closer to the requested delay than 2 cycle (125ns). + */ + +#ifndef __AVR__ + #undef DELAY_NS_ROUND_DOWN + #undef DELAY_NS_ROUND_CLOSEST +#endif + +#if ENABLED(DELAY_NS_ROUND_DOWN) + #define _NS_TO_CYCLES(x) ( (x) * ((F_CPU) / 1000000UL) / 1000UL) // floor +#elif ENABLED(DELAY_NS_ROUND_CLOSEST) + #define _NS_TO_CYCLES(x) (((x) * ((F_CPU) / 1000000UL) + 500) / 1000UL) // round +#else + #define _NS_TO_CYCLES(x) (((x) * ((F_CPU) / 1000000UL) + 999) / 1000UL) // "ceil" +#endif + +#define DELAY_NS(x) DELAY_CYCLES(_NS_TO_CYCLES(x)) +#define DELAY_NS_VAR(x) DELAY_CYCLES_VAR(_NS_TO_CYCLES(x)) diff --git a/src/HAL/shared/HAL.cpp b/src/HAL/shared/HAL.cpp new file mode 100644 index 0000000..4d92aed --- /dev/null +++ b/src/HAL/shared/HAL.cpp @@ -0,0 +1,36 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * HAL/shared/HAL.cpp + */ + +#include "../../inc/MarlinConfig.h" + +MarlinHAL hal; + +#if ENABLED(SOFT_RESET_VIA_SERIAL) + + // Global for use by e_parser.h + void HAL_reboot() { hal.reboot(); } + +#endif diff --git a/src/HAL/shared/HAL_SPI.h b/src/HAL/shared/HAL_SPI.h new file mode 100644 index 0000000..6611f9e --- /dev/null +++ b/src/HAL/shared/HAL_SPI.h @@ -0,0 +1,93 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL/shared/HAL_SPI.h + * Core Marlin definitions for SPI, implemented in the HALs + */ + +#include "Marduino.h" +#include + +/** + * SPI speed where 0 <= index <= 6 + * + * Approximate rates : + * + * 0 : 8 - 10 MHz + * 1 : 4 - 5 MHz + * 2 : 2 - 2.5 MHz + * 3 : 1 - 1.25 MHz + * 4 : 500 - 625 kHz + * 5 : 250 - 312 kHz + * 6 : 125 - 156 kHz + * + * On AVR, actual speed is F_CPU/2^(1 + index). + * On other platforms, speed should be in range given above where possible. + */ + +#define SPI_FULL_SPEED 0 // Set SCK to max rate +#define SPI_HALF_SPEED 1 // Set SCK rate to half of max rate +#define SPI_QUARTER_SPEED 2 // Set SCK rate to quarter of max rate +#define SPI_EIGHTH_SPEED 3 // Set SCK rate to 1/8 of max rate +#define SPI_SIXTEENTH_SPEED 4 // Set SCK rate to 1/16 of max rate +#define SPI_SPEED_5 5 // Set SCK rate to 1/32 of max rate +#define SPI_SPEED_6 6 // Set SCK rate to 1/64 of max rate + +// +// Standard SPI functions +// + +// Initialize SPI bus +void spiBegin(); + +// Configure SPI for specified SPI speed +void spiInit(uint8_t spiRate); + +// Write single byte to SPI +void spiSend(uint8_t b); + +// Read single byte from SPI +uint8_t spiRec(); + +// Read from SPI into buffer +void spiRead(uint8_t *buf, uint16_t nbyte); + +// Write token and then write from 512 byte buffer to SPI (for SD card) +void spiSendBlock(uint8_t token, const uint8_t *buf); + +// Begin SPI transaction, set clock, bit order, data mode +void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode); + +// +// Extended SPI functions taking a channel number (Hardware SPI only) +// + +// Write single byte to specified SPI channel +void spiSend(uint32_t chan, byte b); + +// Write buffer to specified SPI channel +void spiSend(uint32_t chan, const uint8_t *buf, size_t n); + +// Read single byte from specified SPI channel +uint8_t spiRec(uint32_t chan); diff --git a/src/HAL/shared/HAL_ST7920.h b/src/HAL/shared/HAL_ST7920.h new file mode 100644 index 0000000..4e362f9 --- /dev/null +++ b/src/HAL/shared/HAL_ST7920.h @@ -0,0 +1,36 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL/ST7920.h + * For the HALs that provide direct access to the ST7920 display + * (bypassing U8G), it will allow the LIGHTWEIGHT_UI to operate. + */ + +#if BOTH(HAS_MARLINUI_U8GLIB, LIGHTWEIGHT_UI) + void ST7920_cs(); + void ST7920_ncs(); + void ST7920_set_cmd(); + void ST7920_set_dat(); + void ST7920_write_byte(const uint8_t data); +#endif diff --git a/src/HAL/shared/HAL_spi_L6470.cpp b/src/HAL/shared/HAL_spi_L6470.cpp new file mode 100644 index 0000000..5d4ce89 --- /dev/null +++ b/src/HAL/shared/HAL_spi_L6470.cpp @@ -0,0 +1,139 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Software L6470 SPI functions originally from Arduino Sd2Card Library + * Copyright (c) 2009 by William Greiman + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_L64XX + +#include "Delay.h" + +#include "../../core/serial.h" +#include "../../libs/L64XX/L64XX_Marlin.h" + +// Make sure GCC optimizes this file. +// Note that this line triggers a bug in GCC which is fixed by casting. +// See the note below. +#pragma GCC optimize (3) + +// run at ~4Mhz +inline uint8_t L6470_SpiTransfer_Mode_0(uint8_t b) { // using Mode 0 + for (uint8_t bits = 8; bits--;) { + WRITE(L6470_CHAIN_MOSI_PIN, b & 0x80); + b <<= 1; // little setup time + + WRITE(L6470_CHAIN_SCK_PIN, HIGH); + DELAY_NS(125); // 10 cycles @ 84mhz + + b |= (READ(L6470_CHAIN_MISO_PIN) != 0); + + WRITE(L6470_CHAIN_SCK_PIN, LOW); + DELAY_NS(125); // 10 cycles @ 84mhz + } + return b; +} + +inline uint8_t L6470_SpiTransfer_Mode_3(uint8_t b) { // using Mode 3 + for (uint8_t bits = 8; bits--;) { + WRITE(L6470_CHAIN_SCK_PIN, LOW); + WRITE(L6470_CHAIN_MOSI_PIN, b & 0x80); + + DELAY_NS(125); // 10 cycles @ 84mhz + WRITE(L6470_CHAIN_SCK_PIN, HIGH); + DELAY_NS(125); // Need more delay for fast CPUs + + b <<= 1; // little setup time + b |= (READ(L6470_CHAIN_MISO_PIN) != 0); + } + DELAY_NS(125); // 10 cycles @ 84mhz + return b; +} + +/** + * L64XX methods for SPI init and transfer + */ +void L64XX_Marlin::spi_init() { + OUT_WRITE(L6470_CHAIN_SS_PIN, HIGH); + OUT_WRITE(L6470_CHAIN_SCK_PIN, HIGH); + OUT_WRITE(L6470_CHAIN_MOSI_PIN, HIGH); + SET_INPUT(L6470_CHAIN_MISO_PIN); + + #if PIN_EXISTS(L6470_BUSY) + SET_INPUT(L6470_BUSY_PIN); + #endif + + OUT_WRITE(L6470_CHAIN_MOSI_PIN, HIGH); +} + +uint8_t L64XX_Marlin::transfer_single(uint8_t data, int16_t ss_pin) { + // First device in chain has data sent last + extDigitalWrite(ss_pin, LOW); + + hal.isr_off(); // Disable interrupts during SPI transfer (can't allow partial command to chips) + const uint8_t data_out = L6470_SpiTransfer_Mode_3(data); + hal.isr_on(); // Enable interrupts + + extDigitalWrite(ss_pin, HIGH); + return data_out; +} + +uint8_t L64XX_Marlin::transfer_chain(uint8_t data, int16_t ss_pin, uint8_t chain_position) { + uint8_t data_out = 0; + + // first device in chain has data sent last + extDigitalWrite(ss_pin, LOW); + + for (uint8_t i = L64XX::chain[0]; !L64xxManager.spi_abort && i >= 1; i--) { // Send data unless aborted + hal.isr_off(); // Disable interrupts during SPI transfer (can't allow partial command to chips) + const uint8_t temp = L6470_SpiTransfer_Mode_3(uint8_t(i == chain_position ? data : dSPIN_NOP)); + hal.isr_on(); // Enable interrupts + if (i == chain_position) data_out = temp; + } + + extDigitalWrite(ss_pin, HIGH); + return data_out; +} + +/** + * Platform-supplied L6470 buffer transfer method + */ +void L64XX_Marlin::transfer(uint8_t L6470_buf[], const uint8_t length) { + // First device in chain has its data sent last + + if (spi_active) { // Interrupted SPI transfer so need to + WRITE(L6470_CHAIN_SS_PIN, HIGH); // guarantee min high of 650ns + DELAY_US(1); + } + + WRITE(L6470_CHAIN_SS_PIN, LOW); + for (uint8_t i = length; i >= 1; i--) + L6470_SpiTransfer_Mode_3(uint8_t(L6470_buf[i])); + WRITE(L6470_CHAIN_SS_PIN, HIGH); +} + +#pragma GCC reset_options + +#endif // HAS_L64XX diff --git a/src/HAL/shared/Marduino.h b/src/HAL/shared/Marduino.h new file mode 100644 index 0000000..0e2a021 --- /dev/null +++ b/src/HAL/shared/Marduino.h @@ -0,0 +1,96 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * HAL/shared/Marduino.h + */ + +#undef DISABLED // Redefined by ESP32 +#undef M_PI // Redefined by all +#undef _BV // Redefined by some +#undef SBI // Redefined by arduino/const_functions.h +#undef CBI // Redefined by arduino/const_functions.h +#undef sq // Redefined by teensy3/wiring.h +#undef UNUSED // Redefined by stm32f4xx_hal_def.h + +#include // NOTE: If included earlier then this line is a NOOP + +#undef DISABLED +#define DISABLED(V...) DO(DIS,&&,V) + +#undef _BV +#define _BV(b) (1 << (b)) +#ifndef SBI + #define SBI(A,B) (A |= _BV(B)) +#endif +#ifndef CBI + #define CBI(A,B) (A &= ~_BV(B)) +#endif + +#undef sq +#define sq(x) ((x)*(x)) + +#ifndef __AVR__ + #ifndef strchr_P // Some platforms define a macro (DUE, teensy35) + inline const char* strchr_P(const char *s, int c) { return strchr(s,c); } + //#define strchr_P(s,c) strchr(s,c) + #endif + + #ifndef snprintf_P + #define snprintf_P snprintf + #endif + #ifndef vsnprintf_P + #define vsnprintf_P vsnprintf + #endif +#endif + +// Restart causes +#define RST_POWER_ON 1 +#define RST_EXTERNAL 2 +#define RST_BROWN_OUT 4 +#define RST_WATCHDOG 8 +#define RST_JTAG 16 +#define RST_SOFTWARE 32 +#define RST_BACKUP 64 + +#ifndef M_PI + #define M_PI 3.14159265358979323846f +#endif + +// Remove compiler warning on an unused variable +#ifndef UNUSED + #define UNUSED(x) ((void)(x)) +#endif + +#ifndef FORCE_INLINE + #define FORCE_INLINE __attribute__((always_inline)) inline +#endif + +#include "progmem.h" + +class __FlashStringHelper; +typedef const __FlashStringHelper* FSTR_P; +#ifndef FPSTR + #define FPSTR(S) (reinterpret_cast(S)) +#endif +#define FTOP(S) (reinterpret_cast(S)) diff --git a/src/HAL/shared/MinSerial.cpp b/src/HAL/shared/MinSerial.cpp new file mode 100644 index 0000000..2e718d8 --- /dev/null +++ b/src/HAL/shared/MinSerial.cpp @@ -0,0 +1,33 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "MinSerial.h" + +#if ENABLED(POSTMORTEM_DEBUGGING) + +void HAL_min_serial_init_default() {} +void HAL_min_serial_out_default(char ch) { SERIAL_CHAR(ch); } +void (*HAL_min_serial_init)() = &HAL_min_serial_init_default; +void (*HAL_min_serial_out)(char) = &HAL_min_serial_out_default; + +bool MinSerial::force_using_default_output = false; + +#endif diff --git a/src/HAL/shared/MinSerial.h b/src/HAL/shared/MinSerial.h new file mode 100644 index 0000000..3089b8a --- /dev/null +++ b/src/HAL/shared/MinSerial.h @@ -0,0 +1,79 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/serial.h" +#include + +// Serial stuff here +// Inside an exception handler, the CPU state is not safe, we can't expect the handler to resume +// and the software to continue. UART communication can't rely on later callback/interrupt as it might never happen. +// So, you need to provide some method to send one byte to the usual UART with the interrupts disabled +// By default, the method uses SERIAL_CHAR but it's 100% guaranteed to break (couldn't be worse than nothing...)7 +extern void (*HAL_min_serial_init)(); +extern void (*HAL_min_serial_out)(char ch); + +struct MinSerial { + static bool force_using_default_output; + // Serial output + static void TX(char ch) { + if (force_using_default_output) + SERIAL_CHAR(ch); + else + HAL_min_serial_out(ch); + } + // Send String through UART + static void TX(const char *s) { while (*s) TX(*s++); } + // Send a digit through UART + static void TXDigit(uint32_t d) { + if (d < 10) TX((char)(d+'0')); + else if (d < 16) TX((char)(d+'A'-10)); + else TX('?'); + } + + // Send Hex number through UART + static void TXHex(uint32_t v) { + TX("0x"); + for (uint8_t i = 0; i < 8; i++, v <<= 4) + TXDigit((v >> 28) & 0xF); + } + + // Send Decimal number through UART + static void TXDec(uint32_t v) { + if (!v) { + TX('0'); + return; + } + + char nbrs[14]; + char *p = &nbrs[0]; + while (v != 0) { + *p++ = '0' + (v % 10); + v /= 10; + } + do { + p--; + TX(*p); + } while (p != &nbrs[0]); + } + static void init() { if (!force_using_default_output) HAL_min_serial_init(); } +}; diff --git a/src/HAL/shared/backtrace/backtrace.cpp b/src/HAL/shared/backtrace/backtrace.cpp new file mode 100644 index 0000000..33e8e65 --- /dev/null +++ b/src/HAL/shared/backtrace/backtrace.cpp @@ -0,0 +1,106 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#if defined(__arm__) || defined(__thumb__) + +#include "backtrace.h" +#include "unwinder.h" +#include "unwmemaccess.h" + +#include "../MinSerial.h" +#include + +// Dump a backtrace entry +static bool UnwReportOut(void *ctx, const UnwReport *bte) { + int *p = (int*)ctx; + + (*p)++; + + const uint32_t a = bte->address, f = bte->function; + MinSerial::TX('#'); MinSerial::TXDec(*p); MinSerial::TX(" : "); + MinSerial::TX(bte->name?:"unknown"); MinSerial::TX('@'); MinSerial::TXHex(f); + MinSerial::TX('+'); MinSerial::TXDec(a - f); + MinSerial::TX(" PC:"); MinSerial::TXHex(a); + MinSerial::TX('\n'); + return true; +} + +#ifdef UNW_DEBUG + void UnwPrintf(const char *format, ...) { + char dest[256]; + va_list argptr; + va_start(argptr, format); + vsprintf(dest, format, argptr); + va_end(argptr); + MinSerial::TX(&dest[0]); + } +#endif + +/* Table of function pointers for passing to the unwinder */ +static const UnwindCallbacks UnwCallbacks = { + UnwReportOut, + UnwReadW, + UnwReadH, + UnwReadB + #ifdef UNW_DEBUG + , UnwPrintf + #endif +}; + +// Perform a backtrace to the serial port +void backtrace() { + + unsigned long sp = 0, lr = 0, pc = 0; + + // Capture the values of the registers to perform the traceback + __asm__ __volatile__ ( + " mov %[lrv],lr\n" + " mov %[spv],sp\n" + " mov %[pcv],pc\n" + : [spv]"+r"( sp ), + [lrv]"+r"( lr ), + [pcv]"+r"( pc ) + :: + ); + + backtrace_ex(sp, lr, pc); +} + +void backtrace_ex(unsigned long sp, unsigned long lr, unsigned long pc) { + UnwindFrame btf; + + // Fill the traceback structure + btf.sp = sp; + btf.fp = btf.sp; + btf.lr = lr; + btf.pc = pc | 1; // Force Thumb, as CORTEX only support it + + // Perform a backtrace + MinSerial::TX("Backtrace:"); + int ctr = 0; + UnwindStart(&btf, &UnwCallbacks, &ctr); +} + +#else // !__arm__ && !__thumb__ + +void backtrace() {} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/backtrace.h b/src/HAL/shared/backtrace/backtrace.h new file mode 100644 index 0000000..5d2ba60 --- /dev/null +++ b/src/HAL/shared/backtrace/backtrace.h @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// Perform a backtrace to the serial port +void backtrace(); + +// Perform a backtrace to the serial port +void backtrace_ex(unsigned long sp, unsigned long lr, unsigned long pc); diff --git a/src/HAL/shared/backtrace/unwarm.cpp b/src/HAL/shared/backtrace/unwarm.cpp new file mode 100644 index 0000000..adbcca6 --- /dev/null +++ b/src/HAL/shared/backtrace/unwarm.cpp @@ -0,0 +1,175 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Utility functions and glue for ARM unwinding sub-modules. + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + +#define MODULE_NAME "UNWARM" + +#include +#include +#include +#include +#include "unwarm.h" +#include "unwarmmem.h" + +#ifdef UNW_DEBUG + +/** + * Printf wrapper. + * This is used such that alternative outputs for any output can be selected + * by modification of this wrapper function. + */ +void UnwPrintf(const char *format, ...) { + va_list args; + + va_start( args, format ); + vprintf(format, args ); +} +#endif + +/** + * Invalidate all general purpose registers. + */ +void UnwInvalidateRegisterFile(RegData *regFile) { + uint8_t t = 0; + do { + regFile[t].o = REG_VAL_INVALID; + t++; + } while (t < 13); +} + + +/** + * Initialize the data used for unwinding. + */ +void UnwInitState(UnwState * const state, /**< Pointer to structure to fill. */ + const UnwindCallbacks *cb, /**< Callbacks. */ + void *rptData, /**< Data to pass to report function. */ + uint32_t pcValue, /**< PC at which to start unwinding. */ + uint32_t spValue) { /**< SP at which to start unwinding. */ + + UnwInvalidateRegisterFile(state->regData); + + /* Store the pointer to the callbacks */ + state->cb = cb; + state->reportData = rptData; + + /* Setup the SP and PC */ + state->regData[13].v = spValue; + state->regData[13].o = REG_VAL_FROM_CONST; + state->regData[15].v = pcValue; + state->regData[15].o = REG_VAL_FROM_CONST; + + UnwPrintd3("\nInitial: PC=0x%08x SP=0x%08x\n", pcValue, spValue); + + /* Invalidate all memory addresses */ + memset(state->memData.used, 0, sizeof(state->memData.used)); +} + +// Detect if function names are available +static int __attribute__ ((noinline)) has_function_names() { + uint32_t flag_word = ((uint32_t*)(((uint32_t)(&has_function_names)) & (-4))) [-1]; + return ((flag_word & 0xFF000000) == 0xFF000000) ? 1 : 0; +} + +/** + * Call the report function to indicate some return address. + * This returns the value of the report function, which if true + * indicates that unwinding may continue. + */ +bool UnwReportRetAddr(UnwState * const state, uint32_t addr) { + + UnwReport entry; + + // We found two acceptable values. + entry.name = nullptr; + entry.address = addr & 0xFFFFFFFE; // Remove Thumb bit + entry.function = 0; + + // If there are function names, try to solve name + if (has_function_names()) { + + // Lets find the function name, if possible + + // Align address to 4 bytes + uint32_t pf = addr & (-4); + + // Scan backwards until we find the function name + uint32_t v; + while (state->cb->readW(pf-4,&v)) { + + // Check if name descriptor is valid + if ((v & 0xFFFFFF00) == 0xFF000000 && (v & 0xFF) > 1) { + // Assume the name was found! + entry.name = ((const char*)pf) - 4 - (v & 0xFF); + entry.function = pf; + break; + } + + // Go backwards to the previous word + pf -= 4; + } + } + + /* Cast away const from reportData. + * The const is only to prevent the unw module modifying the data. + */ + return state->cb->report((void *)state->reportData, &entry); +} + + +/** + * Write some register to memory. + * This will store some register and meta data onto the virtual stack. + * The address for the write + * \param state [in/out] The unwinding state. + * \param wAddr [in] The address at which to write the data. + * \param reg [in] The register to store. + * \return true if the write was successful, false otherwise. + */ +bool UnwMemWriteRegister(UnwState * const state, const uint32_t addr, const RegData * const reg) { + return UnwMemHashWrite(&state->memData, addr, reg->v, M_IsOriginValid(reg->o)); +} + +/** + * Read a register from memory. + * This will read a register from memory, and setup the meta data. + * If the register has been previously written to memory using + * UnwMemWriteRegister, the local hash will be used to return the + * value while respecting whether the data was valid or not. If the + * register was previously written and was invalid at that point, + * REG_VAL_INVALID will be returned in *reg. + * \param state [in] The unwinding state. + * \param addr [in] The address to read. + * \param reg [out] The result, containing the data value and the origin + * which will be REG_VAL_FROM_MEMORY, or REG_VAL_INVALID. + * \return true if the address could be read and *reg has been filled in. + * false is the data could not be read. + */ +bool UnwMemReadRegister(UnwState * const state, const uint32_t addr, RegData * const reg) { + bool tracked; + + // Check if the value can be found in the hash + if (UnwMemHashRead(&state->memData, addr, ®->v, &tracked)) { + reg->o = tracked ? REG_VAL_FROM_MEMORY : REG_VAL_INVALID; + return true; + } + else if (state->cb->readW(addr, ®->v)) { // Not in the hash, so read from real memory + reg->o = REG_VAL_FROM_MEMORY; + return true; + } + else return false; // Not in the hash, and failed to read from memory +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwarm.h b/src/HAL/shared/backtrace/unwarm.h new file mode 100644 index 0000000..edae906 --- /dev/null +++ b/src/HAL/shared/backtrace/unwarm.h @@ -0,0 +1,140 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Internal interface between the ARM unwinding sub-modules. + **************************************************************************/ + +#pragma once + +#include "unwinder.h" + +/** The maximum number of instructions to interpet in a function. + * Unwinding will be unconditionally stopped and UNWIND_EXHAUSTED returned + * if more than this number of instructions are interpreted in a single + * function without unwinding a stack frame. This prevents infinite loops + * or corrupted program memory from preventing unwinding from progressing. + */ +#define UNW_MAX_INSTR_COUNT 500 + +/** The size of the hash used to track reads and writes to memory. + * This should be a prime value for efficiency. + */ +#define MEM_HASH_SIZE 31 + +/*************************************************************************** + * Type Definitions + **************************************************************************/ + +typedef enum { + /** Invalid value. */ + REG_VAL_INVALID = 0x00, + REG_VAL_FROM_STACK = 0x01, + REG_VAL_FROM_MEMORY = 0x02, + REG_VAL_FROM_CONST = 0x04, + REG_VAL_ARITHMETIC = 0x80 +} RegValOrigin; + + +/** Type for tracking information about a register. + * This stores the register value, as well as other data that helps unwinding. + */ +typedef struct { + + /** The value held in the register. */ + uint32_t v; + + /** The origin of the register value. + * This is used to track how the value in the register was loaded. + */ + int o; /* (RegValOrigin) */ +} RegData; + + +/** Structure used to track reads and writes to memory. + * This structure is used as a hash to store a small number of writes + * to memory. + */ +typedef struct { + /** Memory contents. */ + uint32_t v[MEM_HASH_SIZE]; + + /** Address at which v[n] represents. */ + uint32_t a[MEM_HASH_SIZE]; + + /** Indicates whether the data in v[n] and a[n] is occupied. + * Each bit represents one hash value. + */ + uint8_t used[(MEM_HASH_SIZE + 7) / 8]; + + /** Indicates whether the data in v[n] is valid. + * This allows a[n] to be set, but for v[n] to be marked as invalid. + * Specifically this is needed for when an untracked register value + * is written to memory. + */ + uint8_t tracked[(MEM_HASH_SIZE + 7) / 8]; +} MemData; + + +/** Structure that is used to keep track of unwinding meta-data. + * This data is passed between all the unwinding functions. + */ +typedef struct { + /** The register values and meta-data. */ + RegData regData[16]; + + /** Memory tracking data. */ + MemData memData; + + /** Pointer to the callback functions */ + const UnwindCallbacks *cb; + + /** Pointer to pass to the report function. */ + const void *reportData; +} UnwState; + +/*************************************************************************** + * Macros + **************************************************************************/ + +#define M_IsOriginValid(v) !!((v) & 0x7F) +#define M_Origin2Str(v) ((v) ? "VALID" : "INVALID") + +#ifdef UNW_DEBUG +#define UnwPrintd1(a) state->cb->printf(a) +#define UnwPrintd2(a,b) state->cb->printf(a,b) +#define UnwPrintd3(a,b,c) state->cb->printf(a,b,c) +#define UnwPrintd4(a,b,c,d) state->cb->printf(a,b,c,d) +#define UnwPrintd5(a,b,c,d,e) state->cb->printf(a,b,c,d,e) +#define UnwPrintd6(a,b,c,d,e,f) state->cb->printf(a,b,c,d,e,f) +#define UnwPrintd7(a,b,c,d,e,f,g) state->cb->printf(a,b,c,d,e,f,g) +#define UnwPrintd8(a,b,c,d,e,f,g,h) state->cb->printf(a,b,c,d,e,f,g,h) +#else +#define UnwPrintd1(a) +#define UnwPrintd2(a,b) +#define UnwPrintd3(a,b,c) +#define UnwPrintd4(a,b,c,d) +#define UnwPrintd5(a,b,c,d,e) +#define UnwPrintd6(a,b,c,d,e,f) +#define UnwPrintd7(a,b,c,d,e,f,g) +#define UnwPrintd8(a,b,c,d,e,f,g,h) +#endif + +/*************************************************************************** + * Function Prototypes + **************************************************************************/ + +UnwResult UnwStartArm(UnwState * const state); +UnwResult UnwStartThumb(UnwState * const state); +void UnwInvalidateRegisterFile(RegData *regFile); +void UnwInitState(UnwState * const state, const UnwindCallbacks *cb, void *rptData, uint32_t pcValue, uint32_t spValue); +bool UnwReportRetAddr(UnwState * const state, uint32_t addr); +bool UnwMemWriteRegister(UnwState * const state, const uint32_t addr, const RegData * const reg); +bool UnwMemReadRegister(UnwState * const state, const uint32_t addr, RegData * const reg); +void UnwMemHashGC(UnwState * const state); diff --git a/src/HAL/shared/backtrace/unwarm_arm.cpp b/src/HAL/shared/backtrace/unwarm_arm.cpp new file mode 100644 index 0000000..decf74e --- /dev/null +++ b/src/HAL/shared/backtrace/unwarm_arm.cpp @@ -0,0 +1,534 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Abstract interpreter for ARM mode. + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + +#define MODULE_NAME "UNWARM_ARM" + +#include +#include "unwarm.h" + +/** Check if some instruction is a data-processing instruction. + * Decodes the passed instruction, checks if it is a data-processing and + * verifies that the parameters and operation really indicate a data- + * processing instruction. This is needed because some parts of the + * instruction space under this instruction can be extended or represent + * other operations such as MRS, MSR. + * + * \param[in] inst The instruction word. + * \retval true Further decoding of the instruction indicates that this is + * a valid data-processing instruction. + * \retval false This is not a data-processing instruction, + */ +static bool isDataProc(uint32_t instr) { + uint8_t opcode = (instr & 0x01E00000) >> 21; + if ((instr & 0xFC000000) != 0xE0000000) return false; + + /* TST, TEQ, CMP and CMN all require S to be set */ + bool S = !!(instr & 0x00100000); + if (!S && opcode >= 8 && opcode <= 11) return false; + + return true; +} + +UnwResult UnwStartArm(UnwState * const state) { + uint16_t t = UNW_MAX_INSTR_COUNT; + + for (;;) { + uint32_t instr; + + /* Attempt to read the instruction */ + if (!state->cb->readW(state->regData[15].v, &instr)) + return UNWIND_IREAD_W_FAIL; + + UnwPrintd4("A %x %x %08x:", state->regData[13].v, state->regData[15].v, instr); + + /* Check that the PC is still on Arm alignment */ + if (state->regData[15].v & 0x3) { + UnwPrintd1("\nError: PC misalignment\n"); + return UNWIND_INCONSISTENT; + } + + /* Check that the SP and PC have not been invalidated */ + if (!M_IsOriginValid(state->regData[13].o) || !M_IsOriginValid(state->regData[15].o)) { + UnwPrintd1("\nError: PC or SP invalidated\n"); + return UNWIND_INCONSISTENT; + } + + /* Branch and Exchange (BX) + * This is tested prior to data processing to prevent + * mis-interpretation as an invalid TEQ instruction. + */ + if ((instr & 0xFFFFFFF0) == 0xE12FFF10) { + uint8_t rn = instr & 0xF; + + UnwPrintd4("BX r%d\t ; r%d %s\n", rn, rn, M_Origin2Str(state->regData[rn].o)); + + if (!M_IsOriginValid(state->regData[rn].o)) { + UnwPrintd1("\nUnwind failure: BX to untracked register\n"); + return UNWIND_FAILURE; + } + + /* Set the new PC value */ + state->regData[15].v = state->regData[rn].v; + + /* Check if the return value is from the stack */ + if (state->regData[rn].o == REG_VAL_FROM_STACK) { + + /* Now have the return address */ + UnwPrintd2(" Return PC=%x\n", state->regData[15].v & (~0x1)); + + /* Report the return address */ + if (!UnwReportRetAddr(state, state->regData[rn].v)) + return UNWIND_TRUNCATED; + } + + /* Determine the return mode */ + if (state->regData[rn].v & 0x1) /* Branching to THUMB */ + return UnwStartThumb(state); + + /* Branch to ARM */ + /* Account for the auto-increment which isn't needed */ + state->regData[15].v -= 4; + } + /* Branch */ + else if ((instr & 0xFF000000) == 0xEA000000) { + + int32_t offset = (instr & 0x00FFFFFF) << 2; + + /* Sign extend if needed */ + if (offset & 0x02000000) offset |= 0xFC000000; + + UnwPrintd2("B %d\n", offset); + + /* Adjust PC */ + state->regData[15].v += offset; + + /* Account for pre-fetch, where normally the PC is 8 bytes + * ahead of the instruction just executed. + */ + state->regData[15].v += 4; + } + + /* MRS */ + else if ((instr & 0xFFBF0FFF) == 0xE10F0000) { + uint8_t rd = (instr & 0x0000F000) >> 12; + #ifdef UNW_DEBUG + const bool R = !!(instr & 0x00400000); + UnwPrintd4("MRS r%d,%s\t; r%d invalidated", rd, R ? "SPSR" : "CPSR", rd); + #endif + + /* Status registers untracked */ + state->regData[rd].o = REG_VAL_INVALID; + } + /* MSR */ + else if ((instr & 0xFFB0F000) == 0xE120F000) { + #ifdef UNW_DEBUG + UnwPrintd2("MSR %s_?, ???", (instr & 0x00400000) ? "SPSR" : "CPSR"); + #endif + + /* Status registers untracked. + * Potentially this could change processor mode and switch + * banked registers r8-r14. Most likely is that r13 (sp) will + * be banked. However, invalidating r13 will stop unwinding + * when potentially this write is being used to disable/enable + * interrupts (a common case). Therefore no invalidation is + * performed. + */ + } + /* Data processing */ + else if (isDataProc(instr)) { + bool I = !!(instr & 0x02000000); + uint8_t opcode = (instr & 0x01E00000) >> 21; + #ifdef UNW_DEBUG + bool S = !!(instr & 0x00100000); + #endif + uint8_t rn = (instr & 0x000F0000) >> 16; + uint8_t rd = (instr & 0x0000F000) >> 12; + uint16_t operand2 = (instr & 0x00000FFF); + uint32_t op2val; + int op2origin; + + switch (opcode) { + case 0: UnwPrintd4("AND%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 1: UnwPrintd4("EOR%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 2: UnwPrintd4("SUB%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 3: UnwPrintd4("RSB%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 4: UnwPrintd4("ADD%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 5: UnwPrintd4("ADC%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 6: UnwPrintd4("SBC%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 7: UnwPrintd4("RSC%s r%d,r%d,", S ? "S" : "", rd, rn); break; + case 8: UnwPrintd3("TST%s r%d,", S ? "S" : "", rn); break; + case 9: UnwPrintd3("TEQ%s r%d,", S ? "S" : "", rn); break; + case 10: UnwPrintd3("CMP%s r%d,", S ? "S" : "", rn); break; + case 11: UnwPrintd3("CMN%s r%d,", S ? "S" : "", rn); break; + case 12: UnwPrintd3("ORR%s r%d,", S ? "S" : "", rn); break; + case 13: UnwPrintd3("MOV%s r%d,", S ? "S" : "", rd); break; + case 14: UnwPrintd4("BIC%s r%d,r%d", S ? "S" : "", rd, rn); break; + case 15: UnwPrintd3("MVN%s r%d,", S ? "S" : "", rd); break; + } + + /* Decode operand 2 */ + if (I) { + uint8_t shiftDist = (operand2 & 0x0F00) >> 8; + uint8_t shiftConst = (operand2 & 0x00FF); + + /* rotate const right by 2 * shiftDist */ + shiftDist *= 2; + op2val = (shiftConst >> shiftDist) | + (shiftConst << (32 - shiftDist)); + op2origin = REG_VAL_FROM_CONST; + + UnwPrintd2("#0x%x", op2val); + } + else { + + /* Register and shift */ + uint8_t rm = (operand2 & 0x000F); + uint8_t regShift = !!(operand2 & 0x0010); + uint8_t shiftType = (operand2 & 0x0060) >> 5; + uint32_t shiftDist; + #ifdef UNW_DEBUG + const char * const shiftMnu[4] = { "LSL", "LSR", "ASR", "ROR" }; + #endif + UnwPrintd2("r%d ", rm); + + /* Get the shift distance */ + if (regShift) { + uint8_t rs = (operand2 & 0x0F00) >> 8; + + if (operand2 & 0x00800) { + UnwPrintd1("\nError: Bit should be zero\n"); + return UNWIND_ILLEGAL_INSTR; + } + else if (rs == 15) { + UnwPrintd1("\nError: Cannot use R15 with register shift\n"); + return UNWIND_ILLEGAL_INSTR; + } + + /* Get shift distance */ + shiftDist = state->regData[rs].v; + op2origin = state->regData[rs].o; + + UnwPrintd7("%s r%d\t; r%d %s r%d %s", shiftMnu[shiftType], rs, rm, M_Origin2Str(state->regData[rm].o), rs, M_Origin2Str(state->regData[rs].o)); + } + else { + shiftDist = (operand2 & 0x0F80) >> 7; + op2origin = REG_VAL_FROM_CONST; + if (shiftDist) UnwPrintd3("%s #%d", shiftMnu[shiftType], shiftDist); + UnwPrintd3("\t; r%d %s", rm, M_Origin2Str(state->regData[rm].o)); + } + + /* Apply the shift type to the source register */ + switch (shiftType) { + case 0: /* logical left */ + op2val = state->regData[rm].v << shiftDist; + break; + + case 1: /* logical right */ + if (!regShift && shiftDist == 0) shiftDist = 32; + op2val = state->regData[rm].v >> shiftDist; + break; + + case 2: /* arithmetic right */ + if (!regShift && shiftDist == 0) shiftDist = 32; + + if (state->regData[rm].v & 0x80000000) { + /* Register shifts maybe greater than 32 */ + if (shiftDist >= 32) + op2val = 0xFFFFFFFF; + else + op2val = (state->regData[rm].v >> shiftDist) | (0xFFFFFFFF << (32 - shiftDist)); + } + else + op2val = state->regData[rm].v >> shiftDist; + break; + + case 3: /* rotate right */ + + if (!regShift && shiftDist == 0) { + /* Rotate right with extend. + * This uses the carry bit and so always has an + * untracked result. + */ + op2origin = REG_VAL_INVALID; + op2val = 0; + } + else { + /* Limit shift distance to 0-31 incase of register shift */ + shiftDist &= 0x1F; + + op2val = (state->regData[rm].v >> shiftDist) | + (state->regData[rm].v << (32 - shiftDist)); + } + break; + + default: + UnwPrintd2("\nError: Invalid shift type: %d\n", shiftType); + return UNWIND_FAILURE; + } + + /* Decide the data origin */ + if (M_IsOriginValid(op2origin) && M_IsOriginValid(state->regData[rm].o)) + op2origin = REG_VAL_ARITHMETIC | state->regData[rm].o; + else + op2origin = REG_VAL_INVALID; + } + + /* Propagate register validity */ + switch (opcode) { + case 0: /* AND: Rd := Op1 AND Op2 */ + case 1: /* EOR: Rd := Op1 EOR Op2 */ + case 2: /* SUB: Rd:= Op1 - Op2 */ + case 3: /* RSB: Rd:= Op2 - Op1 */ + case 4: /* ADD: Rd:= Op1 + Op2 */ + case 12: /* ORR: Rd:= Op1 OR Op2 */ + case 14: /* BIC: Rd:= Op1 AND NOT Op2 */ + if (!M_IsOriginValid(state->regData[rn].o) || + !M_IsOriginValid(op2origin)) { + state->regData[rd].o = REG_VAL_INVALID; + } + else { + state->regData[rd].o = state->regData[rn].o; + state->regData[rd].o = (RegValOrigin)(state->regData[rd].o | op2origin); + } + break; + + case 5: /* ADC: Rd:= Op1 + Op2 + C */ + case 6: /* SBC: Rd:= Op1 - Op2 + C */ + case 7: /* RSC: Rd:= Op2 - Op1 + C */ + /* CPSR is not tracked */ + state->regData[rd].o = REG_VAL_INVALID; + break; + + case 8: /* TST: set condition codes on Op1 AND Op2 */ + case 9: /* TEQ: set condition codes on Op1 EOR Op2 */ + case 10: /* CMP: set condition codes on Op1 - Op2 */ + case 11: /* CMN: set condition codes on Op1 + Op2 */ + break; + + case 13: /* MOV: Rd:= Op2 */ + case 15: /* MVN: Rd:= NOT Op2 */ + state->regData[rd].o = (RegValOrigin) op2origin; + break; + } + + /* Account for pre-fetch by temporarily adjusting PC */ + if (rn == 15) { + + /* If the shift amount is specified in the instruction, + * the PC will be 8 bytes ahead. If a register is used + * to specify the shift amount the PC will be 12 bytes + * ahead. + */ + state->regData[rn].v += ((!I && (operand2 & 0x0010)) ? 12 : 8); + } + + /* Compute values */ + switch (opcode) { + case 0: /* AND: Rd := Op1 AND Op2 */ + state->regData[rd].v = state->regData[rn].v & op2val; + break; + + case 1: /* EOR: Rd := Op1 EOR Op2 */ + state->regData[rd].v = state->regData[rn].v ^ op2val; + break; + + case 2: /* SUB: Rd:= Op1 - Op2 */ + state->regData[rd].v = state->regData[rn].v - op2val; + break; + case 3: /* RSB: Rd:= Op2 - Op1 */ + state->regData[rd].v = op2val - state->regData[rn].v; + break; + + case 4: /* ADD: Rd:= Op1 + Op2 */ + state->regData[rd].v = state->regData[rn].v + op2val; + break; + + case 5: /* ADC: Rd:= Op1 + Op2 + C */ + case 6: /* SBC: Rd:= Op1 - Op2 + C */ + case 7: /* RSC: Rd:= Op2 - Op1 + C */ + case 8: /* TST: set condition codes on Op1 AND Op2 */ + case 9: /* TEQ: set condition codes on Op1 EOR Op2 */ + case 10: /* CMP: set condition codes on Op1 - Op2 */ + case 11: /* CMN: set condition codes on Op1 + Op2 */ + UnwPrintd1("\t; ????"); + break; + + case 12: /* ORR: Rd:= Op1 OR Op2 */ + state->regData[rd].v = state->regData[rn].v | op2val; + break; + + case 13: /* MOV: Rd:= Op2 */ + state->regData[rd].v = op2val; + break; + + case 14: /* BIC: Rd:= Op1 AND NOT Op2 */ + state->regData[rd].v = state->regData[rn].v & (~op2val); + break; + + case 15: /* MVN: Rd:= NOT Op2 */ + state->regData[rd].v = ~op2val; + break; + } + + /* Remove the prefetch offset from the PC */ + if (rd != 15 && rn == 15) + state->regData[rn].v -= ((!I && (operand2 & 0x0010)) ? 12 : 8); + } + + /* Block Data Transfer + * LDM, STM + */ + else if ((instr & 0xFE000000) == 0xE8000000) { + + bool P = !!(instr & 0x01000000), + U = !!(instr & 0x00800000), + S = !!(instr & 0x00400000), + W = !!(instr & 0x00200000), + L = !!(instr & 0x00100000); + uint16_t baseReg = (instr & 0x000F0000) >> 16; + uint16_t regList = (instr & 0x0000FFFF); + uint32_t addr = state->regData[baseReg].v; + bool addrValid = M_IsOriginValid(state->regData[baseReg].o); + int8_t r; + + #ifdef UNW_DEBUG + /* Display the instruction */ + if (L) + UnwPrintd6("LDM%c%c r%d%s, {reglist}%s\n", P ? 'E' : 'F', U ? 'D' : 'A', baseReg, W ? "!" : "", S ? "^" : ""); + else + UnwPrintd6("STM%c%c r%d%s, {reglist}%s\n", !P ? 'E' : 'F', !U ? 'D' : 'A', baseReg, W ? "!" : "", S ? "^" : ""); + #endif + + /* S indicates that banked registers (untracked) are used, unless + * this is a load including the PC when the S-bit indicates that + * that CPSR is loaded from SPSR (also untracked, but ignored). + */ + if (S && (!L || (regList & (0x01 << 15)) == 0)) { + UnwPrintd1("\nError:S-bit set requiring banked registers\n"); + return UNWIND_FAILURE; + } + else if (baseReg == 15) { + UnwPrintd1("\nError: r15 used as base register\n"); + return UNWIND_FAILURE; + } + else if (regList == 0) { + UnwPrintd1("\nError: Register list empty\n"); + return UNWIND_FAILURE; + } + + /* Check if ascending or descending. + * Registers are loaded/stored in order of address. + * i.e. r0 is at the lowest address, r15 at the highest. + */ + r = U ? 0 : 15; + do { + + /* Check if the register is to be transferred */ + if (regList & (0x01 << r)) { + + if (P) addr += U ? 4 : -4; + + if (L) { + + if (addrValid) { + + if (!UnwMemReadRegister(state, addr, &state->regData[r])) + return UNWIND_DREAD_W_FAIL; + + /* Update the origin if read via the stack pointer */ + if (M_IsOriginValid(state->regData[r].o) && baseReg == 13) + state->regData[r].o = REG_VAL_FROM_STACK; + + UnwPrintd5(" R%d = 0x%08x\t; r%d %s\n",r,state->regData[r].v,r, M_Origin2Str(state->regData[r].o)); + } + else { + /* Invalidate the register as the base reg was invalid */ + state->regData[r].o = REG_VAL_INVALID; + UnwPrintd2(" R%d = ???\n", r); + } + } + else { + if (addrValid && !UnwMemWriteRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DWRITE_W_FAIL; + + UnwPrintd2(" R%d = 0x%08x\n", r); + } + + if (!P) addr += U ? 4 : -4; + } + + /* Check the next register */ + r += U ? 1 : -1; + + } while (r >= 0 && r <= 15); + + /* Check the writeback bit */ + if (W) state->regData[baseReg].v = addr; + + /* Check if the PC was loaded */ + if (L && (regList & (0x01 << 15))) { + if (!M_IsOriginValid(state->regData[15].o)) { + /* Return address is not valid */ + UnwPrintd1("PC popped with invalid address\n"); + return UNWIND_FAILURE; + } + else { + /* Store the return address */ + if (!UnwReportRetAddr(state, state->regData[15].v)) + return UNWIND_TRUNCATED; + + UnwPrintd2(" Return PC=0x%x", state->regData[15].v); + + /* Determine the return mode */ + if (state->regData[15].v & 0x1) { + /* Branching to THUMB */ + return UnwStartThumb(state); + } + else { + /* Branch to ARM */ + + /* Account for the auto-increment which isn't needed */ + state->regData[15].v -= 4; + } + } + } + } + else { + UnwPrintd1("????"); + + /* Unknown/undecoded. May alter some register, so invalidate file */ + UnwInvalidateRegisterFile(state->regData); + } + + UnwPrintd1("\n"); + + /* Should never hit the reset vector */ + if (state->regData[15].v == 0) return UNWIND_RESET; + + /* Check next address */ + state->regData[15].v += 4; + + /* Garbage collect the memory hash (used only for the stack) */ + UnwMemHashGC(state); + + if (--t == 0) return UNWIND_EXHAUSTED; + + } + + return UNWIND_UNSUPPORTED; +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwarm_thumb.cpp b/src/HAL/shared/backtrace/unwarm_thumb.cpp new file mode 100644 index 0000000..0c6a706 --- /dev/null +++ b/src/HAL/shared/backtrace/unwarm_thumb.cpp @@ -0,0 +1,1066 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Abstract interpretation for Thumb mode. + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + +#define MODULE_NAME "UNWARM_THUMB" + +#include +#include "unwarm.h" + +/** Sign extend an 11 bit value. + * This function simply inspects bit 11 of the input \a value, and if + * set, the top 5 bits are set to give a 2's compliment signed value. + * \param value The value to sign extend. + * \return The signed-11 bit value stored in a 16bit data type. + */ +static int32_t signExtend11(const uint16_t value) { + return (value & 0x400) ? value | 0xFFFFF800 : value; +} + +UnwResult UnwStartThumb(UnwState * const state) { + uint16_t t = UNW_MAX_INSTR_COUNT; + uint32_t lastJumpAddr = 0; // Last JUMP address, to try to detect infinite loops + bool loopDetected = false; // If a loop was detected + + for (;;) { + uint16_t instr; + + /* Attempt to read the instruction */ + if (!state->cb->readH(state->regData[15].v & (~0x1), &instr)) + return UNWIND_IREAD_H_FAIL; + + UnwPrintd4("T %x %x %04x:", state->regData[13].v, state->regData[15].v, instr); + + /* Check that the PC is still on Thumb alignment */ + if (!(state->regData[15].v & 0x1)) { + UnwPrintd1("\nError: PC misalignment\n"); + return UNWIND_INCONSISTENT; + } + + /* Check that the SP and PC have not been invalidated */ + if (!M_IsOriginValid(state->regData[13].o) || !M_IsOriginValid(state->regData[15].o)) { + UnwPrintd1("\nError: PC or SP invalidated\n"); + return UNWIND_INCONSISTENT; + } + + /* + * Detect 32bit thumb instructions + */ + if ((instr & 0xE000) == 0xE000 && (instr & 0x1800) != 0) { + uint16_t instr2; + + /* Check next address */ + state->regData[15].v += 2; + + /* Attempt to read the 2nd part of the instruction */ + if (!state->cb->readH(state->regData[15].v & (~0x1), &instr2)) + return UNWIND_IREAD_H_FAIL; + + UnwPrintd3(" %x %04x:", state->regData[15].v, instr2); + + /* + * Load/Store multiple: Only interpret + * PUSH and POP + */ + if ((instr & 0xFE6F) == 0xE82D) { + bool L = !!(instr & 0x10); + uint16_t rList = instr2; + + if (L) { + uint8_t r; + + /* Load from memory: POP */ + UnwPrintd1("POP {Rlist}\n"); + + /* Load registers from stack */ + for (r = 0; r < 16; r++) { + if (rList & (0x1 << r)) { + + /* Read the word */ + if (!UnwMemReadRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DREAD_W_FAIL; + + /* Alter the origin to be from the stack if it was valid */ + if (M_IsOriginValid(state->regData[r].o)) { + + state->regData[r].o = REG_VAL_FROM_STACK; + + /* If restoring the PC */ + if (r == 15) { + + /* The bottom bit should have been set to indicate that + * the caller was from Thumb. This would allow return + * by BX for interworking APCS. + */ + if ((state->regData[15].v & 0x1) == 0) { + UnwPrintd2("Warning: Return address not to Thumb: 0x%08x\n", state->regData[15].v); + + /* Pop into the PC will not switch mode */ + return UNWIND_INCONSISTENT; + } + + /* Store the return address */ + if (!UnwReportRetAddr(state, state->regData[15].v)) + return UNWIND_TRUNCATED; + + /* Now have the return address */ + UnwPrintd2(" Return PC=%x\n", state->regData[15].v); + + /* Compensate for the auto-increment, which isn't needed here */ + state->regData[15].v -= 2; + + } + + } else { + + if (r == 15) { + /* Return address is not valid */ + UnwPrintd1("PC popped with invalid address\n"); + return UNWIND_FAILURE; + } + } + + state->regData[13].v += 4; + + UnwPrintd3(" r%d = 0x%08x\n", r, state->regData[r].v); + } + } + } + else { + int8_t r; + + /* Store to memory: PUSH */ + UnwPrintd1("PUSH {Rlist}"); + + for (r = 15; r >= 0; r--) { + if (rList & (0x1 << r)) { + UnwPrintd4("\n r%d = 0x%08x\t; %s", r, state->regData[r].v, M_Origin2Str(state->regData[r].o)); + + state->regData[13].v -= 4; + + if (!UnwMemWriteRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DWRITE_W_FAIL; + } + } + } + } + /* + * PUSH register + */ + else if (instr == 0xF84D && (instr2 & 0x0FFF) == 0x0D04) { + uint8_t r = instr2 >> 12; + + /* Store to memory: PUSH */ + UnwPrintd2("PUSH {R%d}\n", r); + UnwPrintd4("\n r%d = 0x%08x\t; %s", r, state->regData[r].v, M_Origin2Str(state->regData[r].o)); + + state->regData[13].v -= 4; + + if (!UnwMemWriteRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DWRITE_W_FAIL; + } + /* + * POP register + */ + else if (instr == 0xF85D && (instr2 & 0x0FFF) == 0x0B04) { + uint8_t r = instr2 >> 12; + + /* Load from memory: POP */ + UnwPrintd2("POP {R%d}\n", r); + + /* Read the word */ + if (!UnwMemReadRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DREAD_W_FAIL; + + /* Alter the origin to be from the stack if it was valid */ + if (M_IsOriginValid(state->regData[r].o)) { + + state->regData[r].o = REG_VAL_FROM_STACK; + + /* If restoring the PC */ + if (r == 15) { + + /* The bottom bit should have been set to indicate that + * the caller was from Thumb. This would allow return + * by BX for interworking APCS. + */ + if ((state->regData[15].v & 0x1) == 0) { + UnwPrintd2("Warning: Return address not to Thumb: 0x%08x\n", state->regData[15].v); + + /* Pop into the PC will not switch mode */ + return UNWIND_INCONSISTENT; + } + + /* Store the return address */ + if (!UnwReportRetAddr(state, state->regData[15].v)) + return UNWIND_TRUNCATED; + + /* Now have the return address */ + UnwPrintd2(" Return PC=%x\n", state->regData[15].v); + + /* Compensate for the auto-increment, which isn't needed here */ + state->regData[15].v -= 2; + + } + + } else { + + if (r == 15) { + /* Return address is not valid */ + UnwPrintd1("PC popped with invalid address\n"); + return UNWIND_FAILURE; + } + } + + state->regData[13].v += 4; + + UnwPrintd3(" r%d = 0x%08x\n", r, state->regData[r].v); + } + /* + * TBB / TBH + */ + else if ((instr & 0xFFF0) == 0xE8D0 && (instr2 & 0xFFE0) == 0xF000) { + /* We are only interested in + * the forms + * TBB [PC, ...] + * TBH [PC, ..., LSL #1] + * as those are used by the C compiler to implement + * the switch clauses + */ + uint8_t rn = instr & 0xF; + bool H = !!(instr2 & 0x10); + + UnwPrintd5("TB%c [r%d,r%d%s]\n", H ? 'H' : 'B', rn, (instr2 & 0xF), H ? ",LSL #1" : ""); + + // We are only interested if the RN is the PC. Let's choose the 1st destination + if (rn == 15) { + if (H) { + uint16_t rv; + if (!state->cb->readH((state->regData[15].v & (~1)) + 2, &rv)) + return UNWIND_DREAD_H_FAIL; + state->regData[15].v += rv * 2; + } + else { + uint8_t rv; + if (!state->cb->readB((state->regData[15].v & (~1)) + 2, &rv)) + return UNWIND_DREAD_B_FAIL; + state->regData[15].v += rv * 2; + } + } + } + /* + * Unconditional branch + */ + else if ((instr & 0xF800) == 0xF000 && (instr2 & 0xD000) == 0x9000) { + uint32_t v; + + uint8_t S = (instr & 0x400) >> 10; + uint16_t imm10 = (instr & 0x3FF); + uint8_t J1 = (instr2 & 0x2000) >> 13; + uint8_t J2 = (instr2 & 0x0800) >> 11; + uint16_t imm11 = (instr2 & 0x7FF); + + uint8_t I1 = J1 ^ S ^ 1; + uint8_t I2 = J2 ^ S ^ 1; + uint32_t imm32 = (S << 24) | (I1 << 23) | (I2 << 22) |(imm10 << 12) | (imm11 << 1); + if (S) imm32 |= 0xFE000000; + + UnwPrintd2("B %d \n", imm32); + + /* Update PC */ + state->regData[15].v += imm32; + + /* Need to advance by a word to account for pre-fetch. + * Advance by a half word here, allowing the normal address + * advance to account for the other half word. + */ + state->regData[15].v += 2; + + /* Compute the jump address */ + v = state->regData[15].v + 2; + + /* Display PC of next instruction */ + UnwPrintd2(" New PC=%x", v); + + /* Did we detect an infinite loop ? */ + loopDetected = lastJumpAddr == v; + + /* Remember the last address we jumped to */ + lastJumpAddr = v; + } + + /* + * Branch with link + */ + else if ((instr & 0xF800) == 0xF000 && (instr2 & 0xD000) == 0xD000) { + + uint8_t S = (instr & 0x400) >> 10; + uint16_t imm10 = (instr & 0x3FF); + uint8_t J1 = (instr2 & 0x2000) >> 13; + uint8_t J2 = (instr2 & 0x0800) >> 11; + uint16_t imm11 = (instr2 & 0x7FF); + + uint8_t I1 = J1 ^ S ^ 1; + uint8_t I2 = J2 ^ S ^ 1; + uint32_t imm32 = (S << 24) | (I1 << 23) | (I2 << 22) |(imm10 << 12) | (imm11 << 1); + if (S) imm32 |= 0xFE000000; + + UnwPrintd2("BL %d \n", imm32); + + /* Never taken, as we are unwinding the stack */ + if (0) { + + /* Store return address in LR register */ + state->regData[14].v = state->regData[15].v + 2; + state->regData[14].o = REG_VAL_FROM_CONST; + + /* Update PC */ + state->regData[15].v += imm32; + + /* Need to advance by a word to account for pre-fetch. + * Advance by a half word here, allowing the normal address + * advance to account for the other half word. + */ + state->regData[15].v += 2; + + /* Display PC of next instruction */ + UnwPrintd2(" Return PC=%x", state->regData[15].v); + + /* Report the return address, including mode bit */ + if (!UnwReportRetAddr(state, state->regData[15].v)) + return UNWIND_TRUNCATED; + + /* Determine the new mode */ + if (state->regData[15].v & 0x1) { + /* Branching to THUMB */ + + /* Account for the auto-increment which isn't needed */ + state->regData[15].v -= 2; + } + else { + /* Branch to ARM */ + return UnwStartArm(state); + } + } + } + + /* + * Conditional branches. Usually not taken, unless infinite loop is detected + */ + else if ((instr & 0xF800) == 0xF000 && (instr2 & 0xD000) == 0x8000) { + + uint8_t S = (instr & 0x400) >> 10; + uint16_t imm6 = (instr & 0x3F); + uint8_t J1 = (instr2 & 0x2000) >> 13; + uint8_t J2 = (instr2 & 0x0800) >> 11; + uint16_t imm11 = (instr2 & 0x7FF); + + uint8_t I1 = J1 ^ S ^ 1; + uint8_t I2 = J2 ^ S ^ 1; + uint32_t imm32 = (S << 20) | (I1 << 19) | (I2 << 18) |(imm6 << 12) | (imm11 << 1); + if (S) imm32 |= 0xFFE00000; + + UnwPrintd2("Bcond %d\n", imm32); + + /* Take the jump only if a loop is detected */ + if (loopDetected) { + + /* Update PC */ + state->regData[15].v += imm32; + + /* Need to advance by a word to account for pre-fetch. + * Advance by a half word here, allowing the normal address + * advance to account for the other half word. + */ + state->regData[15].v += 2; + + /* Display PC of next instruction */ + UnwPrintd2(" New PC=%x", state->regData[15].v + 2); + } + } + /* + * PC-relative load + * LDR Rd,[PC, #+/-imm] + */ + else if ((instr & 0xFF7F) == 0xF85F) { + uint8_t rt = (instr2 & 0xF000) >> 12; + uint8_t imm12 = (instr2 & 0x0FFF); + bool A = !!(instr & 0x80); + uint32_t address; + + /* Compute load address, adding a word to account for prefetch */ + address = (state->regData[15].v & (~0x3)) + 4; + if (A) address += imm12; + else address -= imm12; + + UnwPrintd4("LDR r%d,[PC #%c0x%08x]", rt, A?'+':'-', address); + + if (!UnwMemReadRegister(state, address, &state->regData[rt])) + return UNWIND_DREAD_W_FAIL; + } + /* + * LDR immediate. + * We are only interested when destination is PC. + * LDR Rt,[Rn , #n] + */ + else if ((instr & 0xFFF0) == 0xF8D0) { + uint8_t rn = (instr & 0xF); + uint8_t rt = (instr2 & 0xF000) >> 12; + uint16_t imm12 = (instr2 & 0xFFF); + + /* If destination is PC and we don't know the source value, then fail */ + if (!M_IsOriginValid(state->regData[rn].o)) { + state->regData[rt].o = state->regData[rn].o; + } + else { + uint32_t address = state->regData[rn].v + imm12; + if (!UnwMemReadRegister(state, address, &state->regData[rt])) + return UNWIND_DREAD_W_FAIL; + } + } + /* + * LDR immediate + * We are only interested when destination is PC. + * LDR Rt,[Rn , #-n] + * LDR Rt,[Rn], #+/-n] + * LDR Rt,[Rn, #+/-n]! + */ + else if ((instr & 0xFFF0) == 0xF850 && (instr2 & 0x0800) == 0x0800) { + uint8_t rn = (instr & 0xF); + uint8_t rt = (instr2 & 0xF000) >> 12; + uint16_t imm8 = (instr2 & 0xFF); + bool P = !!(instr2 & 0x400); + bool U = !!(instr2 & 0x200); + bool W = !!(instr2 & 0x100); + + if (!M_IsOriginValid(state->regData[rn].o)) + state->regData[rt].o = state->regData[rn].o; + else { + uint32_t offaddress = state->regData[rn].v + (U ? imm8 + imm8 : 0), + address = P ? offaddress : state->regData[rn].v; + + if (!UnwMemReadRegister(state, address, &state->regData[rt])) + return UNWIND_DREAD_W_FAIL; + + if (W) state->regData[rn].v = offaddress; + } + } + /* + * LDR (register). + * We are interested in the form + * ldr Rt, [Rn, Rm, lsl #x] + * Where Rt is PC, Rn value is known, Rm is not known or unknown + */ + else if ((instr & 0xFFF0) == 0xF850 && (instr2 & 0x0FC0) == 0x0000) { + const uint8_t rn = (instr & 0xF), + rt = (instr2 & 0xF000) >> 12, + rm = (instr2 & 0xF), + imm2 = (instr2 & 0x30) >> 4; + + if (!M_IsOriginValid(state->regData[rn].o) || !M_IsOriginValid(state->regData[rm].o)) { + + /* If Rt is PC, and Rn is known, then do an exception and assume + Rm equals 0 => This takes the first case in a switch() */ + if (rt == 15 && M_IsOriginValid(state->regData[rn].o)) { + uint32_t address = state->regData[rn].v; + if (!UnwMemReadRegister(state, address, &state->regData[rt])) + return UNWIND_DREAD_W_FAIL; + } + else /* Propagate unknown value */ + state->regData[rt].o = state->regData[rn].o; + + } + else { + uint32_t address = state->regData[rn].v + (state->regData[rm].v << imm2); + if (!UnwMemReadRegister(state, address, &state->regData[rt])) + return UNWIND_DREAD_W_FAIL; + } + } + else { + UnwPrintd1("???? (32)"); + + /* Unknown/undecoded. May alter some register, so invalidate file */ + UnwInvalidateRegisterFile(state->regData); + } + /* End of thumb 32bit code */ + + } + /* Format 1: Move shifted register + * LSL Rd, Rs, #Offset5 + * LSR Rd, Rs, #Offset5 + * ASR Rd, Rs, #Offset5 + */ + else if ((instr & 0xE000) == 0x0000 && (instr & 0x1800) != 0x1800) { + bool signExtend; + const uint8_t op = (instr & 0x1800) >> 11, + offset5 = (instr & 0x07C0) >> 6, + rs = (instr & 0x0038) >> 3, + rd = (instr & 0x0007); + + switch (op) { + case 0: /* LSL */ + UnwPrintd6("LSL r%d, r%d, #%d\t; r%d %s", rd, rs, offset5, rs, M_Origin2Str(state->regData[rs].o)); + state->regData[rd].v = state->regData[rs].v << offset5; + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + + case 1: /* LSR */ + UnwPrintd6("LSR r%d, r%d, #%d\t; r%d %s", rd, rs, offset5, rs, M_Origin2Str(state->regData[rs].o)); + state->regData[rd].v = state->regData[rs].v >> offset5; + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + + case 2: /* ASR */ + UnwPrintd6("ASL r%d, r%d, #%d\t; r%d %s", rd, rs, offset5, rs, M_Origin2Str(state->regData[rs].o)); + + signExtend = !!(state->regData[rs].v & 0x8000); + state->regData[rd].v = state->regData[rs].v >> offset5; + if (signExtend) state->regData[rd].v |= 0xFFFFFFFF << (32 - offset5); + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + } + } + /* Format 2: add/subtract + * ADD Rd, Rs, Rn + * ADD Rd, Rs, #Offset3 + * SUB Rd, Rs, Rn + * SUB Rd, Rs, #Offset3 + */ + else if ((instr & 0xF800) == 0x1800) { + bool I = !!(instr & 0x0400); + bool op = !!(instr & 0x0200); + uint8_t rn = (instr & 0x01C0) >> 6; + uint8_t rs = (instr & 0x0038) >> 3; + uint8_t rd = (instr & 0x0007); + + /* Print decoding */ + UnwPrintd6("%s r%d, r%d, %c%d\t;",op ? "SUB" : "ADD",rd, rs,I ? '#' : 'r',rn); + UnwPrintd5("r%d %s, r%d %s",rd, M_Origin2Str(state->regData[rd].o),rs, M_Origin2Str(state->regData[rs].o)); + if (!I) { + + UnwPrintd3(", r%d %s", rn, M_Origin2Str(state->regData[rn].o)); + + /* Perform calculation */ + state->regData[rd].v = state->regData[rs].v + (op ? -state->regData[rn].v : state->regData[rn].v); + + /* Propagate the origin */ + if (M_IsOriginValid(state->regData[rs].o) && M_IsOriginValid(state->regData[rn].o)) { + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + } + else + state->regData[rd].o = REG_VAL_INVALID; + } + else { + /* Perform calculation */ + state->regData[rd].v = state->regData[rs].v + (op ? -rn : rn); + + /* Propagate the origin */ + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + } + } + /* Format 3: move/compare/add/subtract immediate + * MOV Rd, #Offset8 + * CMP Rd, #Offset8 + * ADD Rd, #Offset8 + * SUB Rd, #Offset8 + */ + else if ((instr & 0xE000) == 0x2000) { + + uint8_t op = (instr & 0x1800) >> 11; + uint8_t rd = (instr & 0x0700) >> 8; + uint8_t offset8 = (instr & 0x00FF); + + switch (op) { + case 0: /* MOV */ + UnwPrintd3("MOV r%d, #0x%x", rd, offset8); + state->regData[rd].v = offset8; + state->regData[rd].o = REG_VAL_FROM_CONST; + break; + + case 1: /* CMP */ + /* Irrelevant to unwinding */ + UnwPrintd1("CMP ???"); + break; + + case 2: /* ADD */ + UnwPrintd5("ADD r%d, #0x%x\t; r%d %s", rd, offset8, rd, M_Origin2Str(state->regData[rd].o)); + state->regData[rd].v += offset8; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + + case 3: /* SUB */ + UnwPrintd5("SUB r%d, #0x%d\t; r%d %s", rd, offset8, rd, M_Origin2Str(state->regData[rd].o)); + state->regData[rd].v -= offset8; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + } + } + /* Format 4: ALU operations + * AND Rd, Rs + * EOR Rd, Rs + * LSL Rd, Rs + * LSR Rd, Rs + * ASR Rd, Rs + * ADC Rd, Rs + * SBC Rd, Rs + * ROR Rd, Rs + * TST Rd, Rs + * NEG Rd, Rs + * CMP Rd, Rs + * CMN Rd, Rs + * ORR Rd, Rs + * MUL Rd, Rs + * BIC Rd, Rs + * MVN Rd, Rs + */ + else if ((instr & 0xFC00) == 0x4000) { + uint8_t op = (instr & 0x03C0) >> 6; + uint8_t rs = (instr & 0x0038) >> 3; + uint8_t rd = (instr & 0x0007); + +#ifdef UNW_DEBUG + static const char * const mnu[16] = { + "AND", "EOR", "LSL", "LSR", + "ASR", "ADC", "SBC", "ROR", + "TST", "NEG", "CMP", "CMN", + "ORR", "MUL", "BIC", "MVN" }; +#endif + /* Print the mnemonic and registers */ + switch (op) { + case 0: /* AND */ + case 1: /* EOR */ + case 2: /* LSL */ + case 3: /* LSR */ + case 4: /* ASR */ + case 7: /* ROR */ + case 9: /* NEG */ + case 12: /* ORR */ + case 13: /* MUL */ + case 15: /* MVN */ + UnwPrintd8("%s r%d ,r%d\t; r%d %s, r%d %s",mnu[op],rd, rs, rd, M_Origin2Str(state->regData[rd].o), rs, M_Origin2Str(state->regData[rs].o)); + break; + + case 5: /* ADC */ + case 6: /* SBC */ + UnwPrintd4("%s r%d, r%d", mnu[op], rd, rs); + break; + + case 8: /* TST */ + case 10: /* CMP */ + case 11: /* CMN */ + /* Irrelevant to unwinding */ + UnwPrintd2("%s ???", mnu[op]); + break; + + case 14: /* BIC */ + UnwPrintd5("r%d ,r%d\t; r%d %s", rd, rs, rs, M_Origin2Str(state->regData[rs].o)); + break; + } + + /* Perform operation */ + switch (op) { + case 0: /* AND */ + state->regData[rd].v &= state->regData[rs].v; + break; + + case 1: /* EOR */ + state->regData[rd].v ^= state->regData[rs].v; + break; + + case 2: /* LSL */ + state->regData[rd].v <<= state->regData[rs].v; + break; + + case 3: /* LSR */ + state->regData[rd].v >>= state->regData[rs].v; + break; + + case 4: /* ASR */ + if (state->regData[rd].v & 0x80000000) { + state->regData[rd].v >>= state->regData[rs].v; + state->regData[rd].v |= 0xFFFFFFFF << (32 - state->regData[rs].v); + } + else { + state->regData[rd].v >>= state->regData[rs].v; + } + + break; + + case 5: /* ADC */ + case 6: /* SBC */ + case 8: /* TST */ + case 10: /* CMP */ + case 11: /* CMN */ + break; + + case 7: /* ROR */ + state->regData[rd].v = (state->regData[rd].v >> state->regData[rs].v) | + (state->regData[rd].v << (32 - state->regData[rs].v)); + break; + + case 9: /* NEG */ + state->regData[rd].v = -state->regData[rs].v; + break; + + case 12: /* ORR */ + state->regData[rd].v |= state->regData[rs].v; + break; + + case 13: /* MUL */ + state->regData[rd].v *= state->regData[rs].v; + break; + + case 14: /* BIC */ + state->regData[rd].v &= ~state->regData[rs].v; + break; + + case 15: /* MVN */ + state->regData[rd].v = ~state->regData[rs].v; + break; + } + + /* Propagate data origins */ + switch (op) { + case 0: /* AND */ + case 1: /* EOR */ + case 2: /* LSL */ + case 3: /* LSR */ + case 4: /* ASR */ + case 7: /* ROR */ + case 12: /* ORR */ + case 13: /* MUL */ + case 14: /* BIC */ + if (M_IsOriginValid(state->regData[rd].o) && M_IsOriginValid(state->regData[rs].o)) { + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + } + else + state->regData[rd].o = REG_VAL_INVALID; + break; + + case 5: /* ADC */ + case 6: /* SBC */ + /* C-bit not tracked */ + state->regData[rd].o = REG_VAL_INVALID; + break; + + case 8: /* TST */ + case 10: /* CMP */ + case 11: /* CMN */ + /* Nothing propagated */ + break; + + case 9: /* NEG */ + case 15: /* MVN */ + state->regData[rd].o = state->regData[rs].o; + state->regData[rd].o |= REG_VAL_ARITHMETIC; + break; + } + } + /* Format 5: Hi register operations/branch exchange + * ADD Rd, Hs + * CMP Hd, Rs + * MOV Hd, Hs + */ + else if ((instr & 0xFC00) == 0x4400) { + uint8_t op = (instr & 0x0300) >> 8; + bool h1 = (instr & 0x0080) ? true: false; + bool h2 = (instr & 0x0040) ? true: false; + uint8_t rhs = (instr & 0x0038) >> 3; + uint8_t rhd = (instr & 0x0007); + + /* Adjust the register numbers */ + if (h2) rhs += 8; + if (h1) rhd += 8; + + switch (op) { + case 0: /* ADD */ + UnwPrintd5("ADD r%d, r%d\t; r%d %s", rhd, rhs, rhs, M_Origin2Str(state->regData[rhs].o)); + state->regData[rhd].v += state->regData[rhs].v; + state->regData[rhd].o = state->regData[rhs].o; + state->regData[rhd].o |= REG_VAL_ARITHMETIC; + break; + + case 1: /* CMP */ + /* Irrelevant to unwinding */ + UnwPrintd1("CMP ???"); + break; + + case 2: /* MOV */ + UnwPrintd5("MOV r%d, r%d\t; r%d %s", rhd, rhs, rhd, M_Origin2Str(state->regData[rhs].o)); + state->regData[rhd].v = state->regData[rhs].v; + state->regData[rhd].o = state->regData[rhs].o; + break; + + case 3: /* BX */ + UnwPrintd4("BX r%d\t; r%d %s\n", rhs, rhs, M_Origin2Str(state->regData[rhs].o)); + + /* Only follow BX if the data was from the stack or BX LR */ + if (rhs == 14 || state->regData[rhs].o == REG_VAL_FROM_STACK) { + UnwPrintd2(" Return PC=0x%x\n", state->regData[rhs].v & (~0x1)); + + /* Report the return address, including mode bit */ + if (!UnwReportRetAddr(state, state->regData[rhs].v)) + return UNWIND_TRUNCATED; + + /* Update the PC */ + state->regData[15].v = state->regData[rhs].v; + + /* Determine the new mode */ + if (state->regData[rhs].v & 0x1) { + /* Branching to THUMB */ + + /* Account for the auto-increment which isn't needed */ + state->regData[15].v -= 2; + } + else /* Branch to ARM */ + return UnwStartArm(state); + } + else { + UnwPrintd4("\nError: BX to invalid register: r%d = 0x%x (%s)\n", rhs, state->regData[rhs].o, M_Origin2Str(state->regData[rhs].o)); + return UNWIND_FAILURE; + } + } + } + /* Format 9: PC-relative load + * LDR Rd,[PC, #imm] + */ + else if ((instr & 0xF800) == 0x4800) { + uint8_t rd = (instr & 0x0700) >> 8; + uint8_t word8 = (instr & 0x00FF); + uint32_t address; + + /* Compute load address, adding a word to account for prefetch */ + address = (state->regData[15].v & (~0x3)) + 4 + (word8 << 2); + + UnwPrintd3("LDR r%d, 0x%08x", rd, address); + + if (!UnwMemReadRegister(state, address, &state->regData[rd])) + return UNWIND_DREAD_W_FAIL; + } + /* Format 13: add offset to Stack Pointer + * ADD sp,#+imm + * ADD sp,#-imm + */ + else if ((instr & 0xFF00) == 0xB000) { + uint8_t value = (instr & 0x7F) * 4; + + /* Check the negative bit */ + if ((instr & 0x80) != 0) { + UnwPrintd2("SUB sp,#0x%x", value); + state->regData[13].v -= value; + } + else { + UnwPrintd2("ADD sp,#0x%x", value); + state->regData[13].v += value; + } + } + /* Format 14: push/pop registers + * PUSH {Rlist} + * PUSH {Rlist, LR} + * POP {Rlist} + * POP {Rlist, PC} + */ + else if ((instr & 0xF600) == 0xB400) { + bool L = !!(instr & 0x0800); + bool R = !!(instr & 0x0100); + uint8_t rList = (instr & 0x00FF); + + if (L) { + uint8_t r; + + /* Load from memory: POP */ + UnwPrintd2("POP {Rlist%s}\n", R ? ", PC" : ""); + + for (r = 0; r < 8; r++) { + if (rList & (0x1 << r)) { + + /* Read the word */ + if (!UnwMemReadRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DREAD_W_FAIL; + + /* Alter the origin to be from the stack if it was valid */ + if (M_IsOriginValid(state->regData[r].o)) + state->regData[r].o = REG_VAL_FROM_STACK; + + state->regData[13].v += 4; + + UnwPrintd3(" r%d = 0x%08x\n", r, state->regData[r].v); + } + } + + /* Check if the PC is to be popped */ + if (R) { + /* Get the return address */ + if (!UnwMemReadRegister(state, state->regData[13].v, &state->regData[15])) + return UNWIND_DREAD_W_FAIL; + + /* Alter the origin to be from the stack if it was valid */ + if (!M_IsOriginValid(state->regData[15].o)) { + /* Return address is not valid */ + UnwPrintd1("PC popped with invalid address\n"); + return UNWIND_FAILURE; + } + else { + /* The bottom bit should have been set to indicate that + * the caller was from Thumb. This would allow return + * by BX for interworking APCS. + */ + if ((state->regData[15].v & 0x1) == 0) { + UnwPrintd2("Warning: Return address not to Thumb: 0x%08x\n", state->regData[15].v); + + /* Pop into the PC will not switch mode */ + return UNWIND_INCONSISTENT; + } + + /* Store the return address */ + if (!UnwReportRetAddr(state, state->regData[15].v)) + return UNWIND_TRUNCATED; + + /* Now have the return address */ + UnwPrintd2(" Return PC=%x\n", state->regData[15].v); + + /* Update the pc */ + state->regData[13].v += 4; + + /* Compensate for the auto-increment, which isn't needed here */ + state->regData[15].v -= 2; + } + } + } + else { + int8_t r; + + /* Store to memory: PUSH */ + UnwPrintd2("PUSH {Rlist%s}", R ? ", LR" : ""); + + /* Check if the LR is to be pushed */ + if (R) { + UnwPrintd3("\n lr = 0x%08x\t; %s", state->regData[14].v, M_Origin2Str(state->regData[14].o)); + + state->regData[13].v -= 4; + + /* Write the register value to memory */ + if (!UnwMemWriteRegister(state, state->regData[13].v, &state->regData[14])) + return UNWIND_DWRITE_W_FAIL; + } + + for (r = 7; r >= 0; r--) { + if (rList & (0x1 << r)) { + UnwPrintd4("\n r%d = 0x%08x\t; %s", r, state->regData[r].v, M_Origin2Str(state->regData[r].o)); + + state->regData[13].v -= 4; + + if (!UnwMemWriteRegister(state, state->regData[13].v, &state->regData[r])) + return UNWIND_DWRITE_W_FAIL; + } + } + } + } + + /* + * Conditional branches + * Bcond + */ + else if ((instr & 0xF000) == 0xD000) { + int32_t branchValue = (instr & 0xFF); + if (branchValue & 0x80) branchValue |= 0xFFFFFF00; + + /* Branch distance is twice that specified in the instruction. */ + branchValue *= 2; + + UnwPrintd2("Bcond %d \n", branchValue); + + /* Only take the branch if a loop was detected */ + if (loopDetected) { + + /* Update PC */ + state->regData[15].v += branchValue; + + /* Need to advance by a word to account for pre-fetch. + * Advance by a half word here, allowing the normal address + * advance to account for the other half word. + */ + state->regData[15].v += 2; + + /* Display PC of next instruction */ + UnwPrintd2(" New PC=%x", state->regData[15].v + 2); + } + } + + /* Format 18: unconditional branch + * B label + */ + else if ((instr & 0xF800) == 0xE000) { + uint32_t v; + int32_t branchValue = signExtend11(instr & 0x07FF); + + /* Branch distance is twice that specified in the instruction. */ + branchValue *= 2; + + UnwPrintd2("B %d \n", branchValue); + + /* Update PC */ + state->regData[15].v += branchValue; + + /* Need to advance by a word to account for pre-fetch. + * Advance by a half word here, allowing the normal address + * advance to account for the other half word. + */ + state->regData[15].v += 2; + + /* Compute the jump address */ + v = state->regData[15].v + 2; + + /* Display PC of next instruction */ + UnwPrintd2(" New PC=%x", v); + + /* Did we detect an infinite loop ? */ + loopDetected = lastJumpAddr == v; + + /* Remember the last address we jumped to */ + lastJumpAddr = v; + } + else { + UnwPrintd1("????"); + + /* Unknown/undecoded. May alter some register, so invalidate file */ + UnwInvalidateRegisterFile(state->regData); + } + + UnwPrintd1("\n"); + + /* Should never hit the reset vector */ + if (state->regData[15].v == 0) return UNWIND_RESET; + + /* Check next address */ + state->regData[15].v += 2; + + /* Garbage collect the memory hash (used only for the stack) */ + UnwMemHashGC(state); + + if (--t == 0) return UNWIND_EXHAUSTED; + + } + + return UNWIND_SUCCESS; +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwarmbytab.cpp b/src/HAL/shared/backtrace/unwarmbytab.cpp new file mode 100644 index 0000000..148927a --- /dev/null +++ b/src/HAL/shared/backtrace/unwarmbytab.cpp @@ -0,0 +1,430 @@ +/* + * Libbacktrace + * Copyright 2015 Stephen Street + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://www.mozilla.org/en-US/MPL/2.0/ + * + * This library was modified, some bugs fixed, stack address validated + * and adapted to be used in Marlin 3D printer firmware as backtracer + * for exceptions for debugging purposes in 2018 by Eduardo José Tagle. + */ + +#if defined(__arm__) || defined(__thumb__) + +#include "unwarmbytab.h" + +#include +#include + +/* These symbols point to the unwind index and should be provide by the linker script */ +extern "C" const UnwTabEntry __exidx_start[]; +extern "C" const UnwTabEntry __exidx_end[]; + +/* This prevents the linking of libgcc unwinder code */ +void __aeabi_unwind_cpp_pr0() {}; +void __aeabi_unwind_cpp_pr1() {}; +void __aeabi_unwind_cpp_pr2() {}; + +static inline __attribute__((always_inline)) uint32_t prel31_to_addr(const uint32_t *prel31) { + uint32_t offset = (((uint32_t)(*prel31)) << 1) >> 1; + return ((uint32_t)prel31 + offset) & 0x7FFFFFFF; +} + +static const UnwTabEntry *UnwTabSearchIndex(const UnwTabEntry *start, const UnwTabEntry *end, uint32_t ip) { + const UnwTabEntry *middle; + + /* Perform a binary search of the unwind index */ + while (start < end - 1) { + middle = start + ((end - start + 1) >> 1); + if (ip < prel31_to_addr(&middle->addr_offset)) + end = middle; + else + start = middle; + } + return start; +} + +/* + * Get the function name or nullptr if not found + */ +static const char *UnwTabGetFunctionName(const UnwindCallbacks *cb, uint32_t address) { + uint32_t flag_word = 0; + if (!cb->readW(address-4,&flag_word)) + return nullptr; + + if ((flag_word & 0xFF000000) == 0xFF000000) { + return (const char *)(address - 4 - (flag_word & 0x00FFFFFF)); + } + return nullptr; +} + +/** + * Get the next frame unwinding instruction + * + * Return either the instruction or -1 to signal no more instructions + * are available + */ +static int UnwTabGetNextInstruction(const UnwindCallbacks *cb, UnwTabState *ucb) { + int instruction; + + /* Are there more instructions */ + if (ucb->remaining == 0) + return -1; + + /* Extract the current instruction */ + uint32_t v = 0; + if (!cb->readW(ucb->current, &v)) + return -1; + instruction = (v >> (ucb->byte << 3)) & 0xFF; + + /* Move the next byte */ + --ucb->byte; + if (ucb->byte < 0) { + ucb->current += 4; + ucb->byte = 3; + } + --ucb->remaining; + + return instruction; +} + +/** + * Initialize the frame unwinding state + */ +static UnwResult UnwTabStateInit(const UnwindCallbacks *cb, UnwTabState *ucb, uint32_t instructions, const UnwindFrame *frame) { + + /* Initialize control block */ + memset(ucb, 0, sizeof(UnwTabState)); + ucb->current = instructions; + + /* Is a short unwind description */ + uint32_t v = 0; + if (!cb->readW(instructions, &v)) + return UNWIND_DREAD_W_FAIL; + + if ((v & 0xFF000000) == 0x80000000) { + ucb->remaining = 3; + ucb->byte = 2; + /* Is a long unwind description */ + } else if ((v & 0xFF000000) == 0x81000000) { + ucb->remaining = ((v & 0x00FF0000) >> 14) + 2; + ucb->byte = 1; + } else + return UNWIND_UNSUPPORTED_DWARF_PERSONALITY; + + /* Initialize the virtual register set */ + ucb->vrs[7] = frame->fp; + ucb->vrs[13] = frame->sp; + ucb->vrs[14] = frame->lr; + ucb->vrs[15] = 0; + + /* All good */ + return UNWIND_SUCCESS; +} + +/* + * Execute unwinding instructions + */ +static UnwResult UnwTabExecuteInstructions(const UnwindCallbacks *cb, UnwTabState *ucb) { + int instruction; + uint32_t mask, reg, vsp; + + /* Consume all instruction byte */ + while ((instruction = UnwTabGetNextInstruction(cb, ucb)) != -1) { + + if ((instruction & 0xC0) == 0x00) { // ARM_EXIDX_CMD_DATA_POP + /* vsp += (xxxxxx << 2) + 4 */ + ucb->vrs[13] += ((instruction & 0x3F) << 2) + 4; + } + else if ((instruction & 0xC0) == 0x40) { // ARM_EXIDX_CMD_DATA_PUSH + /* vsp -= (xxxxxx << 2) - 4 */ + ucb->vrs[13] -= ((instruction & 0x3F) << 2) - 4; + } + else if ((instruction & 0xF0) == 0x80) { + /* pop under mask {r15-r12},{r11-r4} or refuse to unwind */ + instruction = instruction << 8 | UnwTabGetNextInstruction(cb, ucb); + + /* Check for refuse to unwind */ + if (instruction == 0x8000) // ARM_EXIDX_CMD_REFUSED + return UNWIND_REFUSED; + + /* Pop registers using mask */ // ARM_EXIDX_CMD_REG_POP + vsp = ucb->vrs[13]; + mask = instruction & 0xFFF; + + reg = 4; + while (mask) { + if ((mask & 1) != 0) { + uint32_t v; + if (!cb->readW(vsp,&v)) + return UNWIND_DREAD_W_FAIL; + ucb->vrs[reg] = v; + v += 4; + } + mask >>= 1; + ++reg; + } + + /* Patch up the vrs sp if it was in the mask */ + if (instruction & (1 << (13 - 4))) + ucb->vrs[13] = vsp; + } + else if ((instruction & 0xF0) == 0x90 // ARM_EXIDX_CMD_REG_TO_SP + && instruction != 0x9D + && instruction != 0x9F + ) { + /* vsp = r[nnnn] */ + ucb->vrs[13] = ucb->vrs[instruction & 0x0F]; + } + else if ((instruction & 0xF0) == 0xA0) { // ARM_EXIDX_CMD_REG_POP + /* pop r4-r[4+nnn] or pop r4-r[4+nnn], r14*/ + vsp = ucb->vrs[13]; + + for (reg = 4; reg <= uint32_t((instruction & 0x07) + 4); ++reg) { + uint32_t v; + if (!cb->readW(vsp,&v)) + return UNWIND_DREAD_W_FAIL; + + ucb->vrs[reg] = v; + vsp += 4; + } + + if (instruction & 0x08) { // ARM_EXIDX_CMD_REG_POP + uint32_t v; + if (!cb->readW(vsp,&v)) + return UNWIND_DREAD_W_FAIL; + ucb->vrs[14] = v; + vsp += 4; + } + + ucb->vrs[13] = vsp; + + } + else if (instruction == 0xB0) { // ARM_EXIDX_CMD_FINISH + /* finished */ + if (ucb->vrs[15] == 0) + ucb->vrs[15] = ucb->vrs[14]; + + /* All done unwinding */ + return UNWIND_SUCCESS; + + } + else if (instruction == 0xB1) { // ARM_EXIDX_CMD_REG_POP + /* pop register under mask {r3,r2,r1,r0} */ + vsp = ucb->vrs[13]; + mask = UnwTabGetNextInstruction(cb, ucb); + + reg = 0; + while (mask) { + if ((mask & 1) != 0) { + uint32_t v; + if (!cb->readW(vsp,&v)) + return UNWIND_DREAD_W_FAIL; + + ucb->vrs[reg] = v; + vsp += 4; + } + mask >>= 1; + ++reg; + } + ucb->vrs[13] = (uint32_t)vsp; + + } + else if (instruction == 0xB2) { // ARM_EXIDX_CMD_DATA_POP + /* vps = vsp + 0x204 + (uleb128 << 2) */ + ucb->vrs[13] += 0x204 + (UnwTabGetNextInstruction(cb, ucb) << 2); + } + else if (instruction == 0xB3 // ARM_EXIDX_CMD_VFP_POP + || instruction == 0xC8 + || instruction == 0xC9 + ) { + /* pop VFP double-precision registers */ + vsp = ucb->vrs[13]; + + /* D[ssss]-D[ssss+cccc] */ + uint32_t v; + if (!cb->readW(vsp,&v)) + return UNWIND_DREAD_W_FAIL; + + ucb->vrs[14] = v; + vsp += 4; + + if (instruction == 0xC8) { + /* D[16+sssss]-D[16+ssss+cccc] */ + ucb->vrs[14] |= 1 << 16; + } + + if (instruction != 0xB3) { + /* D[sssss]-D[ssss+cccc] */ + ucb->vrs[14] |= 1 << 17; + } + + ucb->vrs[13] = vsp; + } + else if ((instruction & 0xF8) == 0xB8 || (instruction & 0xF8) == 0xD0) { + /* Pop VFP double precision registers D[8]-D[8+nnn] */ + ucb->vrs[14] = 0x80 | (instruction & 0x07); + if ((instruction & 0xF8) == 0xD0) + ucb->vrs[14] = 1 << 17; + } + else + return UNWIND_UNSUPPORTED_DWARF_INSTR; + } + return UNWIND_SUCCESS; +} + +static inline __attribute__((always_inline)) uint32_t read_psp() { + /* Read the current PSP and return its value as a pointer */ + uint32_t psp; + + __asm__ volatile ( + " mrs %0, psp \n" + : "=r" (psp) : : + ); + + return psp; +} + +/* + * Unwind the specified frame and goto the previous one + */ +static UnwResult UnwTabUnwindFrame(const UnwindCallbacks *cb, UnwindFrame *frame) { + + UnwResult err; + UnwTabState ucb; + const UnwTabEntry *index; + uint32_t instructions; + + /* Search the unwind index for the matching unwind table */ + index = UnwTabSearchIndex(__exidx_start, __exidx_end, frame->pc); + + /* Make sure we can unwind this frame */ + if (index->insn == 0x00000001) + return UNWIND_SUCCESS; + + /* Get the pointer to the first unwind instruction */ + if (index->insn & 0x80000000) + instructions = (uint32_t)&index->insn; + else + instructions = prel31_to_addr(&index->insn); + + /* Initialize the unwind control block */ + if ((err = UnwTabStateInit(cb, &ucb, instructions, frame)) < 0) + return err; + + /* Execute the unwind instructions */ + err = UnwTabExecuteInstructions(cb, &ucb); + if (err < 0) + return err; + + /* Set the virtual pc to the virtual lr if this is the first unwind */ + if (ucb.vrs[15] == 0) + ucb.vrs[15] = ucb.vrs[14]; + + /* Check for exception return */ + /* TODO Test with other ARM processors to verify this method. */ + if ((ucb.vrs[15] & 0xF0000000) == 0xF0000000) { + /* According to the Cortex Programming Manual (p.44), the stack address is always 8-byte aligned (Cortex-M7). + Depending on where the exception came from (MSP or PSP), we need the right SP value to work with. + + ucb.vrs[7] contains the right value, so take it and align it by 8 bytes, store it as the current + SP to work with (ucb.vrs[13]) which is then saved as the current (virtual) frame's SP. + */ + uint32_t stack; + ucb.vrs[13] = (ucb.vrs[7] & ~7); + + /* If we need to start from the MSP, we need to go down X words to find the PC, where: + X=2 if it was a non-floating-point exception + X=20 if it was a floating-point (VFP) exception + + If we need to start from the PSP, we need to go up exactly 6 words to find the PC. + See the ARMv7-M Architecture Reference Manual p.594 and Cortex-M7 Processor Programming Manual p.44/p.45 for details. + */ + if ((ucb.vrs[15] & 0xC) == 0) { + /* Return to Handler Mode: MSP (0xFFFFFF-1) */ + stack = ucb.vrs[13]; + + /* The PC is always 2 words down from the MSP, if it was a non-floating-point exception */ + stack -= 2*4; + + /* If there was a VFP exception (0xFFFFFFE1), the PC is located another 18 words down */ + if ((ucb.vrs[15] & 0xF0) == 0xE0) { + stack -= 18*4; + } + } + else { + /* Return to Thread Mode: PSP (0xFFFFFF-d) */ + stack = read_psp(); + + /* The PC is always 6 words up from the PSP */ + stack += 6*4; + } + + /* Store the PC */ + uint32_t v; + if (!cb->readW(stack,&v)) + return UNWIND_DREAD_W_FAIL; + ucb.vrs[15] = v; + stack -= 4; + + /* Store the LR */ + if (!cb->readW(stack,&v)) + return UNWIND_DREAD_W_FAIL; + ucb.vrs[14] = v; + stack -= 4; + } + + /* We are done if current frame pc is equal to the virtual pc, prevent infinite loop */ + if (frame->pc == ucb.vrs[15]) + return UNWIND_SUCCESS; + + /* Update the frame */ + frame->fp = ucb.vrs[7]; + frame->sp = ucb.vrs[13]; + frame->lr = ucb.vrs[14]; + frame->pc = ucb.vrs[15]; + + /* All good - Continue unwinding */ + return UNWIND_MORE_AVAILABLE; +} + +UnwResult UnwindByTableStart(UnwindFrame* frame, const UnwindCallbacks *cb, void *data) { + + UnwResult err = UNWIND_SUCCESS; + UnwReport entry; + + /* Use DWARF unwind information to unwind frames */ + do { + if (frame->pc == 0) { + /* Reached __exidx_end. */ + break; + } + + if (frame->pc == 0x00000001) { + /* Reached .cantunwind instruction. */ + break; + } + + /* Find the unwind index of the current frame pc */ + const UnwTabEntry *index = UnwTabSearchIndex(__exidx_start, __exidx_end, frame->pc); + + /* Clear last bit (Thumb indicator) */ + frame->pc &= 0xFFFFFFFEU; + + /* Generate the backtrace information */ + entry.address = frame->pc; + entry.function = prel31_to_addr(&index->addr_offset); + entry.name = UnwTabGetFunctionName(cb, entry.function); + if (!cb->report(data,&entry)) + break; + + /* Unwind frame and repeat */ + } while ((err = UnwTabUnwindFrame(cb, frame)) == UNWIND_MORE_AVAILABLE); + + /* All done */ + return err; +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwarmbytab.h b/src/HAL/shared/backtrace/unwarmbytab.h new file mode 100644 index 0000000..53aeca2 --- /dev/null +++ b/src/HAL/shared/backtrace/unwarmbytab.h @@ -0,0 +1,31 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Interface to the memory tracking sub-system. + **************************************************************************/ + +#pragma once + +#include "unwarm.h" + +typedef struct { + uint32_t vrs[16]; + uint32_t current; /* Address of current byte */ + int remaining; + int byte; +} UnwTabState; + +typedef struct { + uint32_t addr_offset; + uint32_t insn; +} UnwTabEntry; + +UnwResult UnwindByTableStart(UnwindFrame* frame, const UnwindCallbacks *cb, void *data); diff --git a/src/HAL/shared/backtrace/unwarmmem.cpp b/src/HAL/shared/backtrace/unwarmmem.cpp new file mode 100644 index 0000000..2402320 --- /dev/null +++ b/src/HAL/shared/backtrace/unwarmmem.cpp @@ -0,0 +1,106 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Implementation of the memory tracking sub-system. + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) +#define MODULE_NAME "UNWARMMEM" + +#include +#include "unwarmmem.h" +#include "unwarm.h" + +#define M_IsIdxUsed(a, v) !!((a)[v >> 3] & (1 << (v & 0x7))) +#define M_SetIdxUsed(a, v) ((a)[v >> 3] |= (1 << (v & 0x7))) +#define M_ClrIdxUsed(a, v) ((a)[v >> 3] &= ~(1 << (v & 0x7))) + +/** Search the memory hash to see if an entry is stored in the hash already. + * This will search the hash and either return the index where the item is + * stored, or -1 if the item was not found. + */ +static int16_t memHashIndex(MemData * const memData, const uint32_t addr) { + + const uint16_t v = addr % MEM_HASH_SIZE; + uint16_t s = v; + + do { + /* Check if the element is occupied */ + if (M_IsIdxUsed(memData->used, s)) { + /* Check if it is occupied with the sought data */ + if (memData->a[s] == addr) return s; + } + else { + /* Item is free, this is where the item should be stored */ + return s; + } + + /* Search the next entry */ + s++; + if (s > MEM_HASH_SIZE) s = 0; + } while (s != v); + + /* Search failed, hash is full and the address not stored */ + return -1; +} + +bool UnwMemHashRead(MemData * const memData, uint32_t addr,uint32_t * const data, bool * const tracked) { + + const int16_t i = memHashIndex(memData, addr); + + if (i >= 0 && M_IsIdxUsed(memData->used, i) && memData->a[i] == addr) { + *data = memData->v[i]; + *tracked = M_IsIdxUsed(memData->tracked, i); + return true; + } + else { + /* Address not found in the hash */ + return false; + } +} + +bool UnwMemHashWrite(MemData * const memData, uint32_t addr, uint32_t val, bool valValid) { + const int16_t i = memHashIndex(memData, addr); + if (i < 0) return false; /* Hash full */ + + /* Store the item */ + memData->a[i] = addr; + M_SetIdxUsed(memData->used, i); + + if (valValid) { + memData->v[i] = val; + M_SetIdxUsed(memData->tracked, i); + } + else { + #ifdef UNW_DEBUG + memData->v[i] = 0xDEADBEEF; + #endif + M_ClrIdxUsed(memData->tracked, i); + } + + return true; +} + +void UnwMemHashGC(UnwState * const state) { + + const uint32_t minValidAddr = state->regData[13].v; + MemData * const memData = &state->memData; + uint16_t t; + + for (t = 0; t < MEM_HASH_SIZE; t++) { + if (M_IsIdxUsed(memData->used, t) && (memData->a[t] < minValidAddr)) { + UnwPrintd3("MemHashGC: Free elem %d, addr 0x%08x\n", t, memData->a[t]); + M_ClrIdxUsed(memData->used, t); + } + } +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwarmmem.h b/src/HAL/shared/backtrace/unwarmmem.h new file mode 100644 index 0000000..eb4579a --- /dev/null +++ b/src/HAL/shared/backtrace/unwarmmem.h @@ -0,0 +1,21 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Interface to the memory tracking sub-system. + **************************************************************************/ + +#pragma once + +#include "unwarm.h" + +bool UnwMemHashRead(MemData * const memData, uint32_t addr, uint32_t * const data, bool * const tracked); +bool UnwMemHashWrite(MemData * const memData, uint32_t addr, uint32_t val, bool valValid); +void UnwMemHashGC(UnwState * const state); diff --git a/src/HAL/shared/backtrace/unwinder.cpp b/src/HAL/shared/backtrace/unwinder.cpp new file mode 100644 index 0000000..aedfa24 --- /dev/null +++ b/src/HAL/shared/backtrace/unwinder.cpp @@ -0,0 +1,52 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Implementation of the interface into the ARM unwinder. + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + +#define MODULE_NAME "UNWINDER" + +#include +#include +#include "unwinder.h" +#include "unwarm.h" +#include "unwarmbytab.h" + +/* These symbols point to the unwind index and should be provide by the linker script */ +extern "C" const UnwTabEntry __exidx_start[]; +extern "C" const UnwTabEntry __exidx_end[]; + +// Detect if unwind information is present or not +static int HasUnwindTableInfo() { + // > 16 because there are default entries we can't suppress + return ((char*)(&__exidx_end) - (char*)(&__exidx_start)) > 16 ? 1 : 0; +} + +UnwResult UnwindStart(UnwindFrame* frame, const UnwindCallbacks *cb, void *data) { + if (HasUnwindTableInfo()) { + /* We have unwind information tables */ + return UnwindByTableStart(frame, cb, data); + } + else { + + /* We don't have unwind information tables */ + UnwState state; + + /* Initialize the unwinding state */ + UnwInitState(&state, cb, data, frame->pc, frame->sp); + + /* Check the Thumb bit */ + return (frame->pc & 0x1) ? UnwStartThumb(&state) : UnwStartArm(&state); + } +} +#endif diff --git a/src/HAL/shared/backtrace/unwinder.h b/src/HAL/shared/backtrace/unwinder.h new file mode 100644 index 0000000..9280e2f --- /dev/null +++ b/src/HAL/shared/backtrace/unwinder.h @@ -0,0 +1,172 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + **************************************************************************/ +/** \file + * Interface to the ARM stack unwinding module. + **************************************************************************/ + +#pragma once + +#include + +/** \def UNW_DEBUG + * If this define is set, additional information will be produced while + * unwinding the stack to allow debug of the unwind module itself. + */ +/* #define UNW_DEBUG 1 */ + +/*************************************************************************** + * Type Definitions + **************************************************************************/ + +/** Possible results for UnwindStart to return. + */ +typedef enum { + /** Unwinding was successful and complete. */ + UNWIND_SUCCESS = 0, + + /** Not an error: More frames are available. */ + UNWIND_MORE_AVAILABLE = 1, + + /** Unsupported DWARF unwind personality. */ + UNWIND_UNSUPPORTED_DWARF_PERSONALITY = -1, + + /** Refused to perform unwind. */ + UNWIND_REFUSED = -2, + + /** Reached an invalid SP. */ + UNWIND_INVALID_SP = -3, + + /** Reached an invalid PC */ + UNWIND_INVALID_PC = -4, + + /** Unsupported DWARF instruction */ + UNWIND_UNSUPPORTED_DWARF_INSTR = -5, + + /** More than UNW_MAX_INSTR_COUNT instructions were interpreted. */ + UNWIND_EXHAUSTED = -6, + + /** Unwinding stopped because the reporting func returned false. */ + UNWIND_TRUNCATED = -7, + + /** Read data was found to be inconsistent. */ + UNWIND_INCONSISTENT = -8, + + /** Unsupported instruction or data found. */ + UNWIND_UNSUPPORTED = -9, + + /** General failure. */ + UNWIND_FAILURE = -10, + + /** Illegal instruction. */ + UNWIND_ILLEGAL_INSTR = -11, + + /** Unwinding hit the reset vector. */ + UNWIND_RESET = -12, + + /** Failed read for an instruction word. */ + UNWIND_IREAD_W_FAIL = -13, + + /** Failed read for an instruction half-word. */ + UNWIND_IREAD_H_FAIL = -14, + + /** Failed read for an instruction byte. */ + UNWIND_IREAD_B_FAIL = -15, + + /** Failed read for a data word. */ + UNWIND_DREAD_W_FAIL = -16, + + /** Failed read for a data half-word. */ + UNWIND_DREAD_H_FAIL = -17, + + /** Failed read for a data byte. */ + UNWIND_DREAD_B_FAIL = -18, + + /** Failed write for a data word. */ + UNWIND_DWRITE_W_FAIL = -19 + +} UnwResult; + +/** A backtrace report */ +typedef struct { + uint32_t function; /** Starts address of function */ + const char *name; /** Function name, or null if not available */ + uint32_t address; /** PC on that function */ +} UnwReport; + +/** Type for function pointer for result callback. + * The function is passed two parameters, the first is a void * pointer, + * and the second is the return address of the function. The bottom bit + * of the passed address indicates the execution mode; if it is set, + * the execution mode at the return address is Thumb, otherwise it is + * ARM. + * + * The return value of this function determines whether unwinding should + * continue or not. If true is returned, unwinding will continue and the + * report function maybe called again in future. If false is returned, + * unwinding will stop with UnwindStart() returning UNWIND_TRUNCATED. + */ +typedef bool (*UnwindReportFunc)(void *data, const UnwReport *bte); + +/** Structure that holds memory callback function pointers. + */ +typedef struct { + + /** Report an unwind result. */ + UnwindReportFunc report; + + /** Read a 32 bit word from memory. + * The memory address to be read is passed as \a address, and + * \a *val is expected to be populated with the read value. + * If the address cannot or should not be read, false can be + * returned to indicate that unwinding should stop. If true + * is returned, \a *val is assumed to be valid and unwinding + * will continue. + */ + bool (*readW)(const uint32_t address, uint32_t *val); + + /** Read a 16 bit half-word from memory. + * This function has the same usage as for readW, but is expected + * to read only a 16 bit value. + */ + bool (*readH)(const uint32_t address, uint16_t *val); + + /** Read a byte from memory. + * This function has the same usage as for readW, but is expected + * to read only an 8 bit value. + */ + bool (*readB)(const uint32_t address, uint8_t *val); + + #ifdef UNW_DEBUG + /** Print a formatted line for debug. */ + void (*printf)(const char *format, ...); + #endif +} UnwindCallbacks; + +/* A frame */ +typedef struct { + uint32_t fp; + uint32_t sp; + uint32_t lr; + uint32_t pc; +} UnwindFrame; + +/** Start unwinding the current stack. + * This will unwind the stack starting at the PC value supplied to in the + * link register (i.e. not a normal register) and the stack pointer value + * supplied. + * + * -If the program was compiled with -funwind-tables it will use them to + * perform the traceback. Otherwise, brute force will be employed + * -If the program was compiled with -mpoke-function-name, then you will + * get function names in the traceback. Otherwise, you will not. + */ +UnwResult UnwindStart(UnwindFrame* frame, const UnwindCallbacks *cb, void *data); diff --git a/src/HAL/shared/backtrace/unwmemaccess.cpp b/src/HAL/shared/backtrace/unwmemaccess.cpp new file mode 100644 index 0000000..a4151b3 --- /dev/null +++ b/src/HAL/shared/backtrace/unwmemaccess.cpp @@ -0,0 +1,210 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Utility functions to access memory + **************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + +#include "unwmemaccess.h" +#include "../../../inc/MarlinConfig.h" + +/* Validate address */ + +#ifdef ARDUINO_ARCH_SAM + + // For DUE, valid address ranges are + // SRAM (0x20070000 - 0x20088000) (96kb) + // FLASH (0x00080000 - 0x00100000) (512kb) + // + #define START_SRAM_ADDR 0x20070000 + #define END_SRAM_ADDR 0x20088000 + #define START_FLASH_ADDR 0x00080000 + #define END_FLASH_ADDR 0x00100000 + +#elif defined(TARGET_LPC1768) + + // For LPC1769: + // SRAM (0x10000000 - 0x10008000) (32kb) + // FLASH (0x00000000 - 0x00080000) (512kb) + // + #define START_SRAM_ADDR 0x10000000 + #define END_SRAM_ADDR 0x10008000 + #define START_FLASH_ADDR 0x00000000 + #define END_FLASH_ADDR 0x00080000 + +#elif defined(__STM32F1__) || defined(STM32F1xx) || defined(STM32F0xx) || defined(STM32G0xx) + + // For STM32F103ZET6/STM32F103VET6/STM32F0xx + // SRAM (0x20000000 - 0x20010000) (64kb) + // FLASH (0x08000000 - 0x08080000) (512kb) + // + #define START_SRAM_ADDR 0x20000000 + #define END_SRAM_ADDR 0x20010000 + #define START_FLASH_ADDR 0x08000000 + #define END_FLASH_ADDR 0x08080000 + +#elif defined(STM32F4) || defined(STM32F4xx) + + // For STM32F407VET + // SRAM (0x20000000 - 0x20030000) (192kb) + // FLASH (0x08000000 - 0x08080000) (512kb) + // + #define START_SRAM_ADDR 0x20000000 + #define END_SRAM_ADDR 0x20030000 + #define START_FLASH_ADDR 0x08000000 + #define END_FLASH_ADDR 0x08080000 + +#elif MB(REMRAM_V1, NUCLEO_F767ZI) + + // For STM32F765VI in RemRam v1 + // SRAM (0x20000000 - 0x20080000) (512kb) + // FLASH (0x08000000 - 0x08200000) (2048kb) + // + #define START_SRAM_ADDR 0x20000000 + #define END_SRAM_ADDR 0x20080000 + #define START_FLASH_ADDR 0x08000000 + #define END_FLASH_ADDR 0x08200000 + +#elif defined(__MK20DX256__) + + // For MK20DX256 in TEENSY 3.1 or TEENSY 3.2 + // SRAM (0x1FFF8000 - 0x20008000) (64kb) + // FLASH (0x00000000 - 0x00040000) (256kb) + // + #define START_SRAM_ADDR 0x1FFF8000 + #define END_SRAM_ADDR 0x20008000 + #define START_FLASH_ADDR 0x00000000 + #define END_FLASH_ADDR 0x00040000 + +#elif defined(__MK64FX512__) + + // For MK64FX512 in TEENSY 3.5 + // SRAM (0x1FFF0000 - 0x20020000) (192kb) + // FLASH (0x00000000 - 0x00080000) (512kb) + // + #define START_SRAM_ADDR 0x1FFF0000 + #define END_SRAM_ADDR 0x20020000 + #define START_FLASH_ADDR 0x00000000 + #define END_FLASH_ADDR 0x00080000 + +#elif defined(__MK66FX1M0__) + + // For MK66FX1M0 in TEENSY 3.6 + // SRAM (0x1FFF0000 - 0x20030000) (256kb) + // FLASH (0x00000000 - 0x00140000) (1.25Mb) + // + #define START_SRAM_ADDR 0x1FFF0000 + #define END_SRAM_ADDR 0x20030000 + #define START_FLASH_ADDR 0x00000000 + #define END_FLASH_ADDR 0x00140000 + +#elif defined(__IMXRT1062__) + + // For IMXRT1062 in TEENSY 4.0/4/1 + // ITCM (rwx): ORIGIN = 0x00000000, LENGTH = 512K + // DTCM (rwx): ORIGIN = 0x20000000, LENGTH = 512K + // RAM (rwx): ORIGIN = 0x20200000, LENGTH = 512K + // FLASH (rwx): ORIGIN = 0x60000000, LENGTH = 1984K + // + #define START_SRAM_ADDR 0x00000000 + #define END_SRAM_ADDR 0x20280000 + #define START_FLASH_ADDR 0x60000000 + #define END_FLASH_ADDR 0x601F0000 + +#elif defined(__SAMD51P20A__) + + // For SAMD51x20, valid address ranges are + // SRAM (0x20000000 - 0x20040000) (256kb) + // FLASH (0x00000000 - 0x00100000) (1024kb) + // + #define START_SRAM_ADDR 0x20000000 + #define END_SRAM_ADDR 0x20040000 + #define START_FLASH_ADDR 0x00000000 + #define END_FLASH_ADDR 0x00100000 + +#else + // Generic ARM code, that's testing if an access to the given address would cause a fault or not + // It can't guarantee an address is in RAM or Flash only, but we usually don't care + + #define NVIC_FAULT_STAT 0xE000ED28 // Configurable Fault Status Reg. + #define NVIC_CFG_CTRL 0xE000ED14 // Configuration Control Register + #define NVIC_FAULT_STAT_BFARV 0x00008000 // BFAR is valid + #define NVIC_CFG_CTRL_BFHFNMIGN 0x00000100 // Ignore bus fault in NMI/fault + #define HW_REG(X) (*((volatile unsigned long *)(X))) + + static bool validate_addr(uint32_t read_address) { + bool works = true; + uint32_t intDisabled = 0; + // Read current interrupt state + __asm__ __volatile__ ("MRS %[result], PRIMASK\n\t" : [result]"=r"(intDisabled) :: ); // 0 is int enabled, 1 for disabled + + // Clear bus fault indicator first (write 1 to clear) + HW_REG(NVIC_FAULT_STAT) |= NVIC_FAULT_STAT_BFARV; + // Ignore bus fault interrupt + HW_REG(NVIC_CFG_CTRL) |= NVIC_CFG_CTRL_BFHFNMIGN; + // Disable interrupts if not disabled previously + if (!intDisabled) __asm__ __volatile__ ("CPSID f"); + // Probe address + *(volatile uint32_t*)read_address; + // Check if a fault happened + if ((HW_REG(NVIC_FAULT_STAT) & NVIC_FAULT_STAT_BFARV) != 0) + works = false; + // Enable interrupts again if previously disabled + if (!intDisabled) __asm__ __volatile__ ("CPSIE f"); + // Enable fault interrupt flag + HW_REG(NVIC_CFG_CTRL) &= ~NVIC_CFG_CTRL_BFHFNMIGN; + + return works; + } + +#endif + +#ifdef START_SRAM_ADDR + static bool validate_addr(uint32_t addr) { + + // Address must be in SRAM range + if (addr >= START_SRAM_ADDR && addr < END_SRAM_ADDR) + return true; + + // Or in FLASH range + if (addr >= START_FLASH_ADDR && addr < END_FLASH_ADDR) + return true; + + return false; + } +#endif + +bool UnwReadW(const uint32_t a, uint32_t *v) { + if (!validate_addr(a)) + return false; + + *v = *(uint32_t *)a; + return true; +} + +bool UnwReadH(const uint32_t a, uint16_t *v) { + if (!validate_addr(a)) + return false; + + *v = *(uint16_t *)a; + return true; +} + +bool UnwReadB(const uint32_t a, uint8_t *v) { + if (!validate_addr(a)) + return false; + + *v = *(uint8_t *)a; + return true; +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/backtrace/unwmemaccess.h b/src/HAL/shared/backtrace/unwmemaccess.h new file mode 100644 index 0000000..b911e34 --- /dev/null +++ b/src/HAL/shared/backtrace/unwmemaccess.h @@ -0,0 +1,22 @@ +/*************************************************************************** + * ARM Stack Unwinder, Michael.McTernan.2001@cs.bris.ac.uk + * Updated, adapted and several bug fixes on 2018 by Eduardo José Tagle + * + * This program is PUBLIC DOMAIN. + * This means that there is no copyright and anyone is able to take a copy + * for free and use it as they wish, with or without modifications, and in + * any context, commercially or otherwise. The only limitation is that I + * don't guarantee that the software is fit for any purpose or accept any + * liability for its use or misuse - this software is without warranty. + *************************************************************************** + * File Description: Utility functions to access memory + **************************************************************************/ + +#pragma once + +#include "unwarm.h" +#include + +bool UnwReadW(const uint32_t a, uint32_t *v); +bool UnwReadH(const uint32_t a, uint16_t *v); +bool UnwReadB(const uint32_t a, uint8_t *v); diff --git a/src/HAL/shared/cpu_exception/exception_arm.cpp b/src/HAL/shared/cpu_exception/exception_arm.cpp new file mode 100644 index 0000000..e54661c --- /dev/null +++ b/src/HAL/shared/cpu_exception/exception_arm.cpp @@ -0,0 +1,379 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * Copyright (c) 2020 Cyril Russo + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/*************************************************************************** + * ARM CPU Exception handler + ***************************************************************************/ + +#if defined(__arm__) || defined(__thumb__) + + +/* + On ARM CPUs exception handling is quite powerful. + + By default, upon a crash, the CPU enters the handlers that have a higher priority than any other interrupts, + so, in effect, no (real) interrupt can "interrupt" the handler (it's acting like if interrupts were disabled). + + If the handler is not called as re-entrant (that is, if the crash is not happening inside an interrupt or an handler), + then it'll patch the return address to a dumping function (resume_from_fault) and save the crash state. + The CPU will exit the handler and, as such, re-allow the other interrupts, and jump to the dumping function. + In this function, the usual serial port (USB / HW) will be used to dump the crash (no special configuration required). + + The only case where it requires hardware UART is when it's crashing in an interrupt or a crash handler. + In that case, instead of returning to the resume_from_fault function (and thus, re-enabling interrupts), + it jumps to this function directly (so with interrupts disabled), after changing the behavior of the serial output + wrapper to use the HW uart (and in effect, calling MinSerial::init which triggers a warning if you are using + a USB serial port). + + In the case you have a USB serial port, this part will be disabled, and only that part (so that's the reason for + the warning). + This means that you can't have a crash report if the crash happens in an interrupt or an handler if you are using + a USB serial port since it's physically impossible. + You will get a crash report in all other cases. +*/ + +#include "exception_hook.h" +#include "../backtrace/backtrace.h" +#include "../MinSerial.h" + +#define HW_REG(X) (*((volatile unsigned long *)(X))) + +// Default function use the CPU VTOR register to get the vector table. +// Accessing the CPU VTOR register is done in assembly since it's the only way that's common to all current tool +unsigned long get_vtor() { return HW_REG(0xE000ED08); } // Even if it looks like an error, it is not an error +void * hook_get_hardfault_vector_address(unsigned vtor) { return (void*)(vtor + 0x03); } +void * hook_get_memfault_vector_address(unsigned vtor) { return (void*)(vtor + 0x04); } +void * hook_get_busfault_vector_address(unsigned vtor) { return (void*)(vtor + 0x05); } +void * hook_get_usagefault_vector_address(unsigned vtor) { return (void*)(vtor + 0x06); } +void * hook_get_reserved_vector_address(unsigned vtor) { return (void*)(vtor + 0x07); } + +// Common exception frame for ARM, should work for all ARM CPU +// Described here (modified for convenience): https://interrupt.memfault.com/blog/cortex-m-fault-debug +struct __attribute__((packed)) ContextStateFrame { + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t xpsr; +}; + +struct __attribute__((packed)) ContextSavedFrame { + uint32_t R0; + uint32_t R1; + uint32_t R2; + uint32_t R3; + uint32_t R12; + uint32_t LR; + uint32_t PC; + uint32_t XPSR; + + uint32_t CFSR; + uint32_t HFSR; + uint32_t DFSR; + uint32_t AFSR; + uint32_t MMAR; + uint32_t BFAR; + + uint32_t ESP; + uint32_t ELR; +}; + +#if NONE(STM32F0xx, STM32G0xx) + extern "C" + __attribute__((naked)) void CommonHandler_ASM() { + __asm__ __volatile__ ( + // Bit 2 of LR tells which stack pointer to use (either main or process, only main should be used anyway) + "tst lr, #4\n" + "ite eq\n" + "mrseq r0, msp\n" + "mrsne r0, psp\n" + // Save the LR in use when being interrupted + "mov r1, lr\n" + // Get the exception number from the ICSR register + "ldr r2, =0xE000ED00\n" + "ldr r2, [r2, #4]\n" + "b CommonHandler_C\n" + ); + } +#else // Cortex M0 does not support conditional mov and testing with a constant, so let's have a specific handler for it + extern "C" + __attribute__((naked)) void CommonHandler_ASM() { + __asm__ __volatile__ ( + ".syntax unified\n" + // Save the LR in use when being interrupted + "mov r1, lr\n" + // Get the exception number from the ICSR register + "ldr r2, =0xE000ED00\n" + "ldr r2, [r2, #4]\n" + "movs r0, #4\n" + "tst r1, r0\n" + "beq _MSP\n" + "mrs r0, psp\n" + "b CommonHandler_C\n" + "_MSP:\n" + "mrs r0, msp\n" + "b CommonHandler_C\n" + ); + } + + #if DISABLED(DYNAMIC_VECTORTABLE) // Cortex M0 requires the handler's address to be within 32kB to the actual function to be able to branch to it + extern "C" { + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __exc_hardfault(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __exc_busfault(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __exc_usagefault(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __exc_memmanage(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __exc_nmi(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __stm32reservedexception7(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __stm32reservedexception8(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __stm32reservedexception9(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __stm32reservedexception10(); + void __attribute__((naked, alias("CommonHandler_ASM"), nothrow)) __stm32reservedexception13(); + } + //TODO When going off from libmaple, you'll need to replace those by the one from STM32/HAL_MinSerial.cpp + #endif +#endif + +// Must be a macro to avoid creating a function frame +#define HALT_IF_DEBUGGING() \ + do { \ + if (HW_REG(0xE000EDF0) & _BV(0)) { \ + __asm("bkpt 1"); \ + } \ +} while (0) + +// Resume from a fault (if possible) so we can still use interrupt based code for serial output +// In that case, we will not jump back to the faulty code, but instead to a dumping code and then a +// basic loop with watchdog calling or manual resetting +static ContextSavedFrame savedFrame; +static uint8_t lastCause; +bool resume_from_fault() { + static const char* causestr[] = { "Thread", "Rsvd", "NMI", "Hard", "Mem", "Bus", "Usage", "7", "8", "9", "10", "SVC", "Dbg", "13", "PendSV", "SysTk", "IRQ" }; + // Reinit the serial link (might only work if implemented in each of your boards) + MinSerial::init(); + + MinSerial::TX("\n\n## Software Fault detected ##\n"); + MinSerial::TX("Cause: "); MinSerial::TX(causestr[min(lastCause, (uint8_t)16)]); MinSerial::TX('\n'); + + MinSerial::TX("R0 : "); MinSerial::TXHex(savedFrame.R0); MinSerial::TX('\n'); + MinSerial::TX("R1 : "); MinSerial::TXHex(savedFrame.R1); MinSerial::TX('\n'); + MinSerial::TX("R2 : "); MinSerial::TXHex(savedFrame.R2); MinSerial::TX('\n'); + MinSerial::TX("R3 : "); MinSerial::TXHex(savedFrame.R3); MinSerial::TX('\n'); + MinSerial::TX("R12 : "); MinSerial::TXHex(savedFrame.R12); MinSerial::TX('\n'); + MinSerial::TX("LR : "); MinSerial::TXHex(savedFrame.LR); MinSerial::TX('\n'); + MinSerial::TX("PC : "); MinSerial::TXHex(savedFrame.PC); MinSerial::TX('\n'); + MinSerial::TX("PSR : "); MinSerial::TXHex(savedFrame.XPSR); MinSerial::TX('\n'); + + // Configurable Fault Status Register + // Consists of MMSR, BFSR and UFSR + MinSerial::TX("CFSR : "); MinSerial::TXHex(savedFrame.CFSR); MinSerial::TX('\n'); + + // Hard Fault Status Register + MinSerial::TX("HFSR : "); MinSerial::TXHex(savedFrame.HFSR); MinSerial::TX('\n'); + + // Debug Fault Status Register + MinSerial::TX("DFSR : "); MinSerial::TXHex(savedFrame.DFSR); MinSerial::TX('\n'); + + // Auxiliary Fault Status Register + MinSerial::TX("AFSR : "); MinSerial::TXHex(savedFrame.AFSR); MinSerial::TX('\n'); + + // Read the Fault Address Registers. These may not contain valid values. + // Check BFARVALID/MMARVALID to see if they are valid values + // MemManage Fault Address Register + MinSerial::TX("MMAR : "); MinSerial::TXHex(savedFrame.MMAR); MinSerial::TX('\n'); + + // Bus Fault Address Register + MinSerial::TX("BFAR : "); MinSerial::TXHex(savedFrame.BFAR); MinSerial::TX('\n'); + + MinSerial::TX("ExcLR: "); MinSerial::TXHex(savedFrame.ELR); MinSerial::TX('\n'); + MinSerial::TX("ExcSP: "); MinSerial::TXHex(savedFrame.ESP); MinSerial::TX('\n'); + + // The stack pointer is pushed by 8 words upon entering an exception, so we need to revert this + backtrace_ex(savedFrame.ESP + 8*4, savedFrame.LR, savedFrame.PC); + + // Call the last resort function here + hook_last_resort_func(); + + const uint32_t start = millis(), end = start + 100; // 100ms should be enough + // We need to wait for the serial buffers to be output but we don't know for how long + // So we'll just need to refresh the watchdog for a while and then stop for the system to reboot + uint32_t last = start; + while (PENDING(last, end)) { + hal.watchdog_refresh(); + while (millis() == last) { /* nada */ } + last = millis(); + MinSerial::TX('.'); + } + + // Reset now by reinstantiating the bootloader's vector table + HW_REG(0xE000ED08) = 0; + // Restart watchdog + #if DISABLED(USE_WATCHDOG) + // No watchdog, let's perform ARMv7 reset instead by writing to AIRCR register with VECTKEY set to SYSRESETREQ + HW_REG(0xE000ED0C) = (HW_REG(0xE000ED0C) & 0x0000FFFF) | 0x05FA0004; + #endif + + while(1) {} // Bad luck, nothing worked +} + +// Make sure the compiler does not optimize the frame argument away +extern "C" +__attribute__((optimize("O0"))) +void CommonHandler_C(ContextStateFrame * frame, unsigned long lr, unsigned long cause) { + + // If you are using it'll stop here + HALT_IF_DEBUGGING(); + + // Save the state to backtrace later on (don't call memcpy here since the stack can be corrupted) + savedFrame.R0 = frame->r0; + savedFrame.R1 = frame->r1; + savedFrame.R2 = frame->r2; + savedFrame.R3 = frame->r3; + savedFrame.R12 = frame->r12; + savedFrame.LR = frame->lr; + savedFrame.PC = frame->pc; + savedFrame.XPSR= frame->xpsr; + lastCause = cause & 0x1FF; + + volatile uint32_t &CFSR = HW_REG(0xE000ED28); + savedFrame.CFSR = CFSR; + savedFrame.HFSR = HW_REG(0xE000ED2C); + savedFrame.DFSR = HW_REG(0xE000ED30); + savedFrame.AFSR = HW_REG(0xE000ED3C); + savedFrame.MMAR = HW_REG(0xE000ED34); + savedFrame.BFAR = HW_REG(0xE000ED38); + savedFrame.ESP = (unsigned long)frame; // Even on return, this should not be overwritten by the CPU + savedFrame.ELR = lr; + + // First check if we can resume from this exception to our own handler safely + // If we can, then we don't need to disable interrupts and the usual serial code + // can be used + + //const uint32_t non_usage_fault_mask = 0x0000FFFF; + //const bool non_usage_fault_occurred = (CFSR & non_usage_fault_mask) != 0; + // the bottom 8 bits of the xpsr hold the exception number of the + // executing exception or 0 if the processor is in Thread mode + const bool faulted_from_exception = ((frame->xpsr & 0xFF) != 0); + if (!faulted_from_exception) { // Not sure about the non_usage_fault, we want to try anyway, don't we ? && !non_usage_fault_occurred) + // Try to resume to our handler here + CFSR |= CFSR; // The ARM programmer manual says you must write to 1 all fault bits to clear them so this instruction is correct + // The frame will not be valid when returning anymore, let's clean it + savedFrame.CFSR = 0; + + frame->pc = (uint32_t)resume_from_fault; // Patch where to return to + frame->lr = 0xDEADBEEF; // If our handler returns (it shouldn't), let's make it trigger an exception immediately + frame->xpsr = _BV(24); // Need to clean the PSR register to thumb II only + MinSerial::force_using_default_output = true; + return; // The CPU will resume in our handler hopefully, and we'll try to use default serial output + } + + // Sorry, we need to emergency code here since the fault is too dangerous to recover from + MinSerial::force_using_default_output = false; + resume_from_fault(); +} + +void hook_cpu_exceptions() { + #if ENABLED(DYNAMIC_VECTORTABLE) + // On ARM 32bits CPU, the vector table is like this: + // 0x0C => Hardfault + // 0x10 => MemFault + // 0x14 => BusFault + // 0x18 => UsageFault + + // Unfortunately, it's usually run from flash, and we can't write to flash here directly to hook our instruction + // We could set an hardware breakpoint, and hook on the fly when it's being called, but this + // is hard to get right and would probably break debugger when attached + + // So instead, we'll allocate a new vector table filled with the previous value except + // for the fault we are interested in. + // Now, comes the issue to figure out what is the current vector table size + // There is nothing telling us what is the vector table as it's per-cpu vendor specific. + // BUT: we are being called at the end of the setup, so we assume the setup is done + // Thus, we can read the current vector table until we find an address that's not in flash, and it would mark the + // end of the vector table (skipping the fist entry obviously) + // The position of the program in flash is expected to be at 0x08xxx xxxx on all known platform for ARM and the + // flash size is available via register 0x1FFFF7E0 on STM32 family, but it's not the case for all ARM boards + // (accessing this register might trigger a fault if it's not implemented). + + // So we'll simply mask the top 8 bits of the first handler as an hint of being in the flash or not -that's poor and will + // probably break if the flash happens to be more than 128MB, but in this case, we are not magician, we need help from outside. + + unsigned long *vecAddr = (unsigned long*)get_vtor(); + SERIAL_ECHOPGM("Vector table addr: "); + SERIAL_PRINTLN(get_vtor(), PrintBase::Hex); + + #ifdef VECTOR_TABLE_SIZE + uint32_t vec_size = VECTOR_TABLE_SIZE; + alignas(128) static unsigned long vectable[VECTOR_TABLE_SIZE] ; + #else + #ifndef IS_IN_FLASH + #define IS_IN_FLASH(X) (((unsigned long)X & 0xFF000000) == 0x08000000) + #endif + + // When searching for the end of the vector table, this acts as a limit not to overcome + #ifndef VECTOR_TABLE_SENTINEL + #define VECTOR_TABLE_SENTINEL 80 + #endif + + // Find the vector table size + uint32_t vec_size = 1; + while (IS_IN_FLASH(vecAddr[vec_size]) && vec_size < VECTOR_TABLE_SENTINEL) + vec_size++; + + // We failed to find a valid vector table size, let's abort hooking up + if (vec_size == VECTOR_TABLE_SENTINEL) return; + // Poor method that's wasting RAM here, but allocating with malloc and alignment would be worst + // 128 bytes alignment is required for writing the VTOR register + alignas(128) static unsigned long vectable[VECTOR_TABLE_SENTINEL]; + + SERIAL_ECHOPGM("Detected vector table size: "); + SERIAL_PRINTLN(vec_size, PrintBase::Hex); + #endif + + uint32_t defaultFaultHandler = vecAddr[(unsigned)7]; + // Copy the current vector table into the new table + for (uint32_t i = 0; i < vec_size; i++) { + vectable[i] = vecAddr[i]; + // Replace all default handler by our own handler + if (vectable[i] == defaultFaultHandler) + vectable[i] = (unsigned long)&CommonHandler_ASM; + } + + // Let's hook now with our functions + vectable[(unsigned long)hook_get_hardfault_vector_address(0)] = (unsigned long)&CommonHandler_ASM; + vectable[(unsigned long)hook_get_memfault_vector_address(0)] = (unsigned long)&CommonHandler_ASM; + vectable[(unsigned long)hook_get_busfault_vector_address(0)] = (unsigned long)&CommonHandler_ASM; + vectable[(unsigned long)hook_get_usagefault_vector_address(0)] = (unsigned long)&CommonHandler_ASM; + + // Finally swap with our own vector table + // This is supposed to be atomic, but let's do that with interrupt disabled + + HW_REG(0xE000ED08) = (unsigned long)vectable | _BV32(29); // 29th bit is for telling the CPU the table is now in SRAM (should be present already) + + SERIAL_ECHOLNPGM("Installed fault handlers"); + #endif +} + +#endif // __arm__ || __thumb__ diff --git a/src/HAL/shared/cpu_exception/exception_hook.cpp b/src/HAL/shared/cpu_exception/exception_hook.cpp new file mode 100644 index 0000000..93e80f3 --- /dev/null +++ b/src/HAL/shared/cpu_exception/exception_hook.cpp @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "exception_hook.h" + +void * __attribute__((weak)) hook_get_hardfault_vector_address(unsigned) { return 0; } +void * __attribute__((weak)) hook_get_memfault_vector_address(unsigned) { return 0; } +void * __attribute__((weak)) hook_get_busfault_vector_address(unsigned) { return 0; } +void * __attribute__((weak)) hook_get_usagefault_vector_address(unsigned) { return 0; } +void __attribute__((weak)) hook_last_resort_func() {} diff --git a/src/HAL/shared/cpu_exception/exception_hook.h b/src/HAL/shared/cpu_exception/exception_hook.h new file mode 100644 index 0000000..70d9434 --- /dev/null +++ b/src/HAL/shared/cpu_exception/exception_hook.h @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/* Here is the expected behavior of a system producing a CPU exception with this hook installed: + 1. Before the system is crashed + 1.1 Upon validation (not done yet in this code, but we could be using DEBUG flags here to allow/disallow hooking) + 1.2 Install the hook by overwriting the vector table exception handler with the hooked function + 2. Upon system crash (for example, by a dereference of a NULL pointer or anything else) + 2.1 The CPU triggers its exception and jump into the vector table for the exception type + 2.2 Instead of finding the default handler, it finds the updated pointer to our hook + 2.3 The CPU jumps into our hook function (likely a naked function to keep all information about crash point intact) + 2.4 The hook (naked) function saves the important registers (stack pointer, program counter, current mode) and jumps to a common exception handler (in C) + 2.5 The common exception handler dumps the registers on the serial link and perform a backtrace around the crashing point + 2.6 Once the backtrace is performed the last resort function is called (platform specific). + On some platform with a LCD screen, this might display the crash information as a QR code or as text for the + user to capture by taking a picture + 2.7 The CPU is reset and/or halted by triggering a debug breakpoint if a debugger is attached */ + +// Hook into CPU exception interrupt table to call the backtracing code upon an exception +// Most platform will simply do nothing here, but those who can will install/overwrite the default exception handler +// with a more performant exception handler +void hook_cpu_exceptions(); + +// Some platform might deal without a hard fault handler, in that case, return 0 in your platform here or skip implementing it +void * __attribute__((weak)) hook_get_hardfault_vector_address(unsigned base_address); +// Some platform might deal without a memory management fault handler, in that case, return 0 in your platform here or skip implementing it +void * __attribute__((weak)) hook_get_memfault_vector_address(unsigned base_address); +// Some platform might deal without a bus fault handler, in that case, return 0 in your platform here or skip implementing it +void * __attribute__((weak)) hook_get_busfault_vector_address(unsigned base_address); +// Some platform might deal without a usage fault handler, in that case, return 0 in your platform here or skip implementing it +void * __attribute__((weak)) hook_get_usagefault_vector_address(unsigned base_address); + +// Last resort function that can be called after the exception handler was called. +void __attribute__((weak)) hook_last_resort_func(); diff --git a/src/HAL/shared/eeprom_api.cpp b/src/HAL/shared/eeprom_api.cpp new file mode 100644 index 0000000..47cfa5a --- /dev/null +++ b/src/HAL/shared/eeprom_api.cpp @@ -0,0 +1,30 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#include "../../inc/MarlinConfigPre.h" + +#if EITHER(EEPROM_SETTINGS, SD_FIRMWARE_UPDATE) + + #include "eeprom_api.h" + PersistentStore persistentStore; + +#endif diff --git a/src/HAL/shared/eeprom_api.h b/src/HAL/shared/eeprom_api.h new file mode 100644 index 0000000..cd744f8 --- /dev/null +++ b/src/HAL/shared/eeprom_api.h @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include +#include + +#include "../../libs/crc16.h" + +class PersistentStore { +public: + + // Total available persistent storage space (in bytes) + static size_t capacity(); + + // Prepare to read or write + static bool access_start(); + + // Housecleaning after read or write + static bool access_finish(); + + // Write one or more bytes of data and update the CRC + // Return 'true' on write error + static bool write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc); + + // Read one or more bytes of data and update the CRC + // Return 'true' on read error + static bool read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing=true); + + // Write one or more bytes of data + // Return 'true' on write error + static bool write_data(const int pos, const uint8_t *value, const size_t size=sizeof(uint8_t)) { + int data_pos = pos; + uint16_t crc = 0; + return write_data(data_pos, value, size, &crc); + } + + // Write a single byte of data + // Return 'true' on write error + static bool write_data(const int pos, const uint8_t value) { return write_data(pos, &value); } + + // Read one or more bytes of data + // Return 'true' on read error + static bool read_data(const int pos, uint8_t *value, const size_t size=1) { + int data_pos = pos; + uint16_t crc = 0; + return read_data(data_pos, value, size, &crc); + } +}; + +extern PersistentStore persistentStore; diff --git a/src/HAL/shared/eeprom_if.h b/src/HAL/shared/eeprom_if.h new file mode 100644 index 0000000..e496de2 --- /dev/null +++ b/src/HAL/shared/eeprom_if.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +// +// EEPROM +// +void eeprom_init(); +void eeprom_write_byte(uint8_t *pos, uint8_t value); +uint8_t eeprom_read_byte(uint8_t *pos); diff --git a/src/HAL/shared/eeprom_if_i2c.cpp b/src/HAL/shared/eeprom_if_i2c.cpp new file mode 100644 index 0000000..6b559e2 --- /dev/null +++ b/src/HAL/shared/eeprom_if_i2c.cpp @@ -0,0 +1,102 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Platform-independent Arduino functions for I2C EEPROM. + * Enable USE_SHARED_EEPROM if not supplied by the framework. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(I2C_EEPROM) + +#include "eeprom_if.h" + +#if ENABLED(SOFT_I2C_EEPROM) + #include + SlowSoftWire Wire = SlowSoftWire(I2C_SDA_PIN, I2C_SCL_PIN, true); +#else + #include +#endif + +void eeprom_init() { + Wire.begin( + #if PINS_EXIST(I2C_SCL, I2C_SDA) && DISABLED(SOFT_I2C_EEPROM) + uint8_t(I2C_SDA_PIN), uint8_t(I2C_SCL_PIN) + #endif + ); +} + +#if ENABLED(USE_SHARED_EEPROM) + +#ifndef EEPROM_WRITE_DELAY + #define EEPROM_WRITE_DELAY 5 +#endif +#ifndef EEPROM_DEVICE_ADDRESS + #define EEPROM_DEVICE_ADDRESS 0x50 +#endif + +static constexpr uint8_t eeprom_device_address = I2C_ADDRESS(EEPROM_DEVICE_ADDRESS); + +// ------------------------ +// Public functions +// ------------------------ + +#define SMALL_EEPROM (MARLIN_EEPROM_SIZE <= 2048) + +// Combine Address high bits into the device address on <=16Kbit (2K) and >512Kbit (64K) EEPROMs. +// Note: MARLIN_EEPROM_SIZE is specified in bytes, whereas EEPROM model numbers refer to bits. +// e.g., The "16" in BL24C16 indicates a 16Kbit (2KB) size. +static uint8_t _eeprom_calc_device_address(uint8_t * const pos) { + const unsigned eeprom_address = (unsigned)pos; + return (SMALL_EEPROM || MARLIN_EEPROM_SIZE > 65536) + ? uint8_t(eeprom_device_address | ((eeprom_address >> (SMALL_EEPROM ? 8 : 16)) & 0x07)) + : eeprom_device_address; +} + +static void _eeprom_begin(uint8_t * const pos) { + const unsigned eeprom_address = (unsigned)pos; + Wire.beginTransmission(_eeprom_calc_device_address(pos)); + if (!SMALL_EEPROM) + Wire.write(uint8_t((eeprom_address >> 8) & 0xFF)); // Address High, if needed + Wire.write(uint8_t(eeprom_address & 0xFF)); // Address Low +} + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + _eeprom_begin(pos); + Wire.write(value); + Wire.endTransmission(); + + // wait for write cycle to complete + // this could be done more efficiently with "acknowledge polling" + delay(EEPROM_WRITE_DELAY); +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + _eeprom_begin(pos); + Wire.endTransmission(); + Wire.requestFrom(_eeprom_calc_device_address(pos), (byte)1); + return Wire.available() ? Wire.read() : 0xFF; +} + +#endif // USE_SHARED_EEPROM +#endif // I2C_EEPROM diff --git a/src/HAL/shared/eeprom_if_spi.cpp b/src/HAL/shared/eeprom_if_spi.cpp new file mode 100644 index 0000000..72c35ad --- /dev/null +++ b/src/HAL/shared/eeprom_if_spi.cpp @@ -0,0 +1,84 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * Platform-independent Arduino functions for SPI EEPROM. + * Enable USE_SHARED_EEPROM if not supplied by the framework. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SPI_EEPROM) + +#include "eeprom_if.h" + +void eeprom_init() {} + +#if ENABLED(USE_SHARED_EEPROM) + +#define CMD_WREN 6 // WREN +#define CMD_READ 2 // WRITE +#define CMD_WRITE 2 // WRITE + +#ifndef EEPROM_WRITE_DELAY + #define EEPROM_WRITE_DELAY 7 +#endif + +static void _eeprom_begin(uint8_t * const pos, const uint8_t cmd) { + const uint8_t eeprom_temp[3] = { + cmd, + (unsigned(pos) >> 8) & 0xFF, // Address High + unsigned(pos) & 0xFF // Address Low + }; + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Usually free already + WRITE(SPI_EEPROM1_CS_PIN, LOW); // Activate the Bus + spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3); + // Leave the Bus in-use +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + _eeprom_begin(pos, CMD_READ); // Set read location and begin transmission + + const uint8_t v = spiRec(SPI_CHAN_EEPROM1); // After READ a value sits on the Bus + + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with device + + return v; +} + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + const uint8_t eeprom_temp = CMD_WREN; + WRITE(SPI_EEPROM1_CS_PIN, LOW); + spiSend(SPI_CHAN_EEPROM1, &eeprom_temp, 1); // Write Enable + + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with the Bus + delay(1); // For a small amount of time + + _eeprom_begin(pos, CMD_WRITE); // Set write address and begin transmission + + spiSend(SPI_CHAN_EEPROM1, value); // Send the value to be written + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with the Bus + delay(EEPROM_WRITE_DELAY); // Give page write time to complete +} + +#endif // USE_SHARED_EEPROM +#endif // I2C_EEPROM diff --git a/src/HAL/shared/esp_wifi.cpp b/src/HAL/shared/esp_wifi.cpp new file mode 100644 index 0000000..a55f5ca --- /dev/null +++ b/src/HAL/shared/esp_wifi.cpp @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" +#include "Delay.h" + +void esp_wifi_init(void) { // init ESP01 WIFI module pins + #if PIN_EXISTS(ESP_WIFI_MODULE_GPIO0) + OUT_WRITE(ESP_WIFI_MODULE_GPIO0_PIN, HIGH); + #endif + #if PIN_EXISTS(ESP_WIFI_MODULE_GPIO2) + OUT_WRITE(ESP_WIFI_MODULE_GPIO2_PIN, HIGH); + #endif + #if PIN_EXISTS(ESP_WIFI_MODULE_RESET) + delay(1); // power up delay (0.1mS minimum) + OUT_WRITE(ESP_WIFI_MODULE_RESET_PIN, LOW); + delay(1); + OUT_WRITE(ESP_WIFI_MODULE_RESET_PIN, HIGH); + #endif + #if PIN_EXISTS(ESP_WIFI_MODULE_ENABLE) + delay(1); // delay after reset released (0.1mS minimum) + OUT_WRITE(ESP_WIFI_MODULE_ENABLE_PIN, HIGH); + #endif +} diff --git a/src/HAL/shared/esp_wifi.h b/src/HAL/shared/esp_wifi.h new file mode 100644 index 0000000..84a50a9 --- /dev/null +++ b/src/HAL/shared/esp_wifi.h @@ -0,0 +1,24 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +void esp_wifi_init(); diff --git a/src/HAL/shared/math_32bit.h b/src/HAL/shared/math_32bit.h new file mode 100644 index 0000000..1fb233e --- /dev/null +++ b/src/HAL/shared/math_32bit.h @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#include "../../core/macros.h" + +/** + * Math helper functions for 32 bit CPUs + */ +FORCE_INLINE static uint32_t MultiU32X24toH32(uint32_t longIn1, uint32_t longIn2) { + return ((uint64_t)longIn1 * longIn2 + 0x00800000) >> 24; +} diff --git a/src/HAL/shared/progmem.h b/src/HAL/shared/progmem.h new file mode 100644 index 0000000..4cd7663 --- /dev/null +++ b/src/HAL/shared/progmem.h @@ -0,0 +1,190 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +#ifndef __AVR__ +#ifndef __PGMSPACE_H_ +// This define should prevent reading the system pgmspace.h if included elsewhere +// This is not normally needed. +#define __PGMSPACE_H_ 1 +#endif + +#ifndef PROGMEM +#define PROGMEM +#endif +#ifndef PGM_P +#define PGM_P const char * +#endif +#ifndef PSTR +#define PSTR(str) (str) +#endif +#ifndef F +class __FlashStringHelper; +#define F(str) (reinterpret_cast(PSTR(str))) +#endif +#ifndef _SFR_BYTE +#define _SFR_BYTE(n) (n) +#endif +#ifndef memchr_P +#define memchr_P(str, c, len) memchr((str), (c), (len)) +#endif +#ifndef memcmp_P +#define memcmp_P(a, b, n) memcmp((a), (b), (n)) +#endif +#ifndef memcpy_P +#define memcpy_P(dest, src, num) memcpy((dest), (src), (num)) +#endif +#ifndef memmem_P +#define memmem_P(a, alen, b, blen) memmem((a), (alen), (b), (blen)) +#endif +#ifndef memrchr_P +#define memrchr_P(str, val, len) memrchr((str), (val), (len)) +#endif +#ifndef strcat_P +#define strcat_P(dest, src) strcat((dest), (src)) +#endif +#ifndef strchr_P +#define strchr_P(str, c) strchr((str), (c)) +#endif +#ifndef strchrnul_P +#define strchrnul_P(str, c) strchrnul((str), (c)) +#endif +#ifndef strcmp_P +#define strcmp_P(a, b) strcmp((a), (b)) +#endif +#ifndef strcpy_P +#define strcpy_P(dest, src) strcpy((dest), (src)) +#endif +#ifndef strcasecmp_P +#define strcasecmp_P(a, b) strcasecmp((a), (b)) +#endif +#ifndef strcasestr_P +#define strcasestr_P(a, b) strcasestr((a), (b)) +#endif +#ifndef strlcat_P +#define strlcat_P(dest, src, len) strlcat((dest), (src), (len)) +#endif +#ifndef strlcpy_P +#define strlcpy_P(dest, src, len) strlcpy((dest), (src), (len)) +#endif +#ifndef strlen_P +#define strlen_P(s) strlen((const char *)(s)) +#endif +#ifndef strnlen_P +#define strnlen_P(str, len) strnlen((str), (len)) +#endif +#ifndef strncmp_P +#define strncmp_P(a, b, n) strncmp((a), (b), (n)) +#endif +#ifndef strncasecmp_P +#define strncasecmp_P(a, b, n) strncasecmp((a), (b), (n)) +#endif +#ifndef strncat_P +#define strncat_P(a, b, n) strncat((a), (b), (n)) +#endif +#ifndef strncpy_P +#define strncpy_P(a, b, n) strncpy((a), (b), (n)) +#endif +#ifndef strpbrk_P +#define strpbrk_P(str, chrs) strpbrk((str), (chrs)) +#endif +#ifndef strrchr_P +#define strrchr_P(str, c) strrchr((str), (c)) +#endif +#ifndef strsep_P +#define strsep_P(pstr, delim) strsep((pstr), (delim)) +#endif +#ifndef strspn_P +#define strspn_P(str, chrs) strspn((str), (chrs)) +#endif +#ifndef strstr_P +#define strstr_P(a, b) strstr((a), (b)) +#endif +#ifndef sprintf_P +#define sprintf_P(s, ...) sprintf((s), __VA_ARGS__) +#endif +#ifndef vfprintf_P +#define vfprintf_P(s, ...) vfprintf((s), __VA_ARGS__) +#endif +#ifndef printf_P +#define printf_P(...) printf(__VA_ARGS__) +#endif +#ifndef snprintf_P +#define snprintf_P(s, n, ...) snprintf((s), (n), __VA_ARGS__) +#endif +#ifndef vsprintf_P +#define vsprintf_P(s, ...) vsprintf((s),__VA_ARGS__) +#endif +#ifndef vsnprintf_P +#define vsnprintf_P(s, n, ...) vsnprintf((s), (n),__VA_ARGS__) +#endif +#ifndef fprintf_P +#define fprintf_P(s, ...) fprintf((s), __VA_ARGS__) +#endif + +#ifndef pgm_read_byte +#define pgm_read_byte(addr) (*(const unsigned char *)(addr)) +#endif +#ifndef pgm_read_word +#define pgm_read_word(addr) (*(const unsigned short *)(addr)) +#endif +#ifndef pgm_read_dword +#define pgm_read_dword(addr) (*(const unsigned long *)(addr)) +#endif +#ifndef pgm_read_float +#define pgm_read_float(addr) (*(const float *)(addr)) +#endif + +#ifndef pgm_read_byte_near +#define pgm_read_byte_near(addr) pgm_read_byte(addr) +#endif +#ifndef pgm_read_word_near +#define pgm_read_word_near(addr) pgm_read_word(addr) +#endif +#ifndef pgm_read_dword_near +#define pgm_read_dword_near(addr) pgm_read_dword(addr) +#endif +#ifndef pgm_read_float_near +#define pgm_read_float_near(addr) pgm_read_float(addr) +#endif +#ifndef pgm_read_byte_far +#define pgm_read_byte_far(addr) pgm_read_byte(addr) +#endif +#ifndef pgm_read_word_far +#define pgm_read_word_far(addr) pgm_read_word(addr) +#endif +#ifndef pgm_read_dword_far +#define pgm_read_dword_far(addr) pgm_read_dword(addr) +#endif +#ifndef pgm_read_float_far +#define pgm_read_float_far(addr) pgm_read_float(addr) +#endif + +#ifndef pgm_read_pointer +#define pgm_read_pointer +#endif + +// Fix bug in pgm_read_ptr +#undef pgm_read_ptr +#define pgm_read_ptr(addr) (*((void**)(addr))) + +#endif // __AVR__ diff --git a/src/HAL/shared/servo.cpp b/src/HAL/shared/servo.cpp new file mode 100644 index 0000000..b838800 --- /dev/null +++ b/src/HAL/shared/servo.cpp @@ -0,0 +1,157 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * servo.cpp - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + */ + +/** + * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. + * The servos are pulsed in the background using the value most recently written using the write() method + * + * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. + * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. + * + * The methods are: + * + * Servo - Class for manipulating servo motors connected to Arduino pins. + * + * attach(pin) - Attach a servo motor to an i/o pin. + * attach(pin, min, max) - Attach to a pin, setting min and max values in microseconds + * Default min is 544, max is 2400 + * + * write() - Set the servo angle in degrees. (Invalid angles —over MIN_PULSE_WIDTH— are treated as µs.) + * writeMicroseconds() - Set the servo pulse width in microseconds. + * move(pin, angle) - Sequence of attach(pin), write(angle), safe_delay(servo_delay[servoIndex]). + * With DEACTIVATE_SERVOS_AFTER_MOVE it detaches after servo_delay[servoIndex]. + * read() - Get the last-written servo pulse width as an angle between 0 and 180. + * readMicroseconds() - Get the last-written servo pulse width in microseconds. + * attached() - Return true if a servo is attached. + * detach() - Stop an attached servo from pulsing its i/o pin. + */ + +#include "../../inc/MarlinConfig.h" + +#if SHARED_SERVOS + +#include "servo.h" +#include "servo_private.h" + +ServoInfo_t servo_info[MAX_SERVOS]; // static array of servo info structures +uint8_t ServoCount = 0; // the total number of attached servos + +#define SERVO_MIN(v) (MIN_PULSE_WIDTH - (v) * 4) // minimum value in uS for this servo +#define SERVO_MAX(v) (MAX_PULSE_WIDTH - (v) * 4) // maximum value in uS for this servo + +/************ static functions common to all instances ***********************/ + +static bool anyTimerChannelActive(const timer16_Sequence_t timer) { + // returns true if any servo is active on this timer + LOOP_L_N(channel, SERVOS_PER_TIMER) { + if (SERVO(timer, channel).Pin.isActive) + return true; + } + return false; +} + +/****************** end of static functions ******************************/ + +Servo::Servo() { + if (ServoCount < MAX_SERVOS) { + servoIndex = ServoCount++; // assign a servo index to this instance + servo_info[servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009 + } + else + servoIndex = INVALID_SERVO; // too many servos +} + +int8_t Servo::attach(const int inPin) { + return attach(inPin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH); +} + +int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { + + if (servoIndex >= MAX_SERVOS) return -1; + + if (inPin > 0) servo_info[servoIndex].Pin.nbr = inPin; + pinMode(servo_info[servoIndex].Pin.nbr, OUTPUT); // set servo pin to output + + // TODO: min/max check: ABS(min - MIN_PULSE_WIDTH) / 4 < 128 + min = (MIN_PULSE_WIDTH - inMin) / 4; //resolution of min/max is 4 uS + max = (MAX_PULSE_WIDTH - inMax) / 4; + + // initialize the timer if it has not already been initialized + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (!anyTimerChannelActive(timer)) initISR(timer); + servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive + + return servoIndex; +} + +void Servo::detach() { + servo_info[servoIndex].Pin.isActive = false; + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (!anyTimerChannelActive(timer)) finISR(timer); + //pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input +} + +void Servo::write(int value) { + if (value < MIN_PULSE_WIDTH) // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds) + value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(min), SERVO_MAX(max)); + writeMicroseconds(value); +} + +void Servo::writeMicroseconds(int value) { + // calculate and store the values for the given channel + byte channel = servoIndex; + if (channel < MAX_SERVOS) { // ensure channel is valid + // ensure pulse width is valid + value = constrain(value, SERVO_MIN(min), SERVO_MAX(max)) - (TRIM_DURATION); + value = usToTicks(value); // convert to ticks after compensating for interrupt overhead - 12 Aug 2009 + + CRITICAL_SECTION_START(); + servo_info[channel].ticks = value; + CRITICAL_SECTION_END(); + } +} + +// return the value as degrees +int Servo::read() { return map(readMicroseconds() + 1, SERVO_MIN(min), SERVO_MAX(max), 0, 180); } + +int Servo::readMicroseconds() { + return (servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[servoIndex].ticks) + (TRIM_DURATION); +} + +bool Servo::attached() { return servo_info[servoIndex].Pin.isActive; } + +void Servo::move(const int value) { + constexpr uint16_t servo_delay[] = SERVO_DELAY; + static_assert(COUNT(servo_delay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM_SERVOS long."); + if (attach(0) >= 0) { + write(value); + safe_delay(servo_delay[servoIndex]); + TERN_(DEACTIVATE_SERVOS_AFTER_MOVE, detach()); + } +} + +#endif // SHARED_SERVOS diff --git a/src/HAL/shared/servo.h b/src/HAL/shared/servo.h new file mode 100644 index 0000000..c2560a8 --- /dev/null +++ b/src/HAL/shared/servo.h @@ -0,0 +1,115 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method. + * The servos are pulsed in the background using the value most recently written using the write() method + * + * Note that analogWrite of PWM on pins associated with the timer are disabled when the first servo is attached. + * Timers are seized as needed in groups of 12 servos - 24 servos use two timers, 48 servos will use four. + * The sequence used to seize timers is defined in timers.h + * + * The methods are: + * + * Servo - Class for manipulating servo motors connected to Arduino pins. + * + * attach(pin ) - Attaches a servo motor to an i/o pin. + * attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds + * default min is 544, max is 2400 + * + * write() - Sets the servo angle in degrees. (invalid angle that is valid as pulse in microseconds is treated as microseconds) + * writeMicroseconds() - Sets the servo pulse width in microseconds + * read() - Gets the last written servo pulse width as an angle between 0 and 180. + * readMicroseconds() - Gets the last written servo pulse width in microseconds. (was read_us() in first release) + * attached() - Returns true if there is a servo attached. + * detach() - Stops an attached servos from pulsing its i/o pin. + * move(angle) - Sequence of attach(0), write(angle), + * With DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DELAY and detach. + */ + +#if IS_TEENSY32 + #include "../TEENSY31_32/Servo.h" +#elif IS_TEENSY35 || IS_TEENSY36 + #include "../TEENSY35_36/Servo.h" +#elif IS_TEENSY40 || IS_TEENSY41 + #include "../TEENSY40_41/Servo.h" +#elif defined(TARGET_LPC1768) + #include "../LPC1768/Servo.h" +#elif defined(__STM32F1__) || defined(TARGET_STM32F1) + #include "../STM32F1/Servo.h" +#elif defined(ARDUINO_ARCH_STM32) + #include "../STM32/Servo.h" +#elif defined(ARDUINO_ARCH_ESP32) + #include "../ESP32/Servo.h" +#else + #include + + #if defined(__AVR__) || defined(ARDUINO_ARCH_SAM) || defined(__SAMD51__) + // we're good to go + #else + #error "This library only supports boards with an AVR, SAM3X or SAMD51 processor." + #endif + + #define Servo_VERSION 2 // software version of this library + + class Servo { + public: + Servo(); + int8_t attach(const int pin); // attach the given pin to the next free channel, set pinMode, return channel number (-1 on fail) + int8_t attach(const int pin, const int min, const int max); // as above but also sets min and max values for writes. + void detach(); + void write(int value); // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds + void writeMicroseconds(int value); // write pulse width in microseconds + void move(const int value); // attach the servo, then move to value + // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds + // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DELAY, then detach + int read(); // returns current pulse width as an angle between 0 and 180 degrees + int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) + bool attached(); // return true if this servo is attached, otherwise false + + private: + uint8_t servoIndex; // index into the channel data for this servo + int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH + int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH + }; + +#endif diff --git a/src/HAL/shared/servo_private.h b/src/HAL/shared/servo_private.h new file mode 100644 index 0000000..10cc5a1 --- /dev/null +++ b/src/HAL/shared/servo_private.h @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ +#pragma once + +/** + * servo_private.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include + +// Architecture specific include +#ifdef __AVR__ + #include "../AVR/ServoTimers.h" +#elif defined(ARDUINO_ARCH_SAM) + #include "../DUE/ServoTimers.h" +#elif defined(__SAMD51__) + #include "../SAMD51/ServoTimers.h" +#else + #error "This library only supports boards with an AVR, SAM3X or SAMD51 processor." +#endif + +// Macros + +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds + +#define SERVOS_PER_TIMER 12 // the maximum number of servos controlled by one timer +#define MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER) + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + +// Convert microseconds to ticks and back (PRESCALER depends on architecture) +#define usToTicks(_us) (clockCyclesPerMicrosecond() * (_us) / (SERVO_TIMER_PRESCALER)) +#define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond()) + +// convenience macros +#define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel +#define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel + +// Types + +typedef struct { + uint8_t nbr : 7 ; // a pin number from 0 to 127 + uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t; + +typedef struct { + ServoPin_t Pin; + unsigned int ticks; +} ServoInfo_t; + +// Global variables + +extern uint8_t ServoCount; +extern ServoInfo_t servo_info[MAX_SERVOS]; + +// Public functions + +void initISR(const timer16_Sequence_t timer_index); +void finISR(const timer16_Sequence_t timer_index); diff --git a/src/gcode/bedlevel/G26.cpp b/src/gcode/bedlevel/G26.cpp new file mode 100644 index 0000000..aa6e0c1 --- /dev/null +++ b/src/gcode/bedlevel/G26.cpp @@ -0,0 +1,876 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * G26 Mesh Validation Tool + * + * G26 is a Mesh Validation Tool intended to provide support for the Marlin Unified Bed Leveling System. + * In order to fully utilize and benefit from the Marlin Unified Bed Leveling System an accurate Mesh must + * be defined. G29 is designed to allow the user to quickly validate the correctness of her Mesh. It will + * first heat the bed and nozzle. It will then print lines and circles along the Mesh Cell boundaries and + * the intersections of those lines (respectively). + * + * This action allows the user to immediately see where the Mesh is properly defined and where it needs to + * be edited. The command will generate the Mesh lines closest to the nozzle's starting position. Alternatively + * the user can specify the X and Y position of interest with command parameters. This allows the user to + * focus on a particular area of the Mesh where attention is needed. + * + * B # Bed Set the Bed Temperature. If not specified, a default of 60 C. will be assumed. + * + * C Current When searching for Mesh Intersection points to draw, use the current nozzle location + * as the base for any distance comparison. + * + * D Disable Disable the Unified Bed Leveling System. In the normal case the user is invoking this + * command to see how well a Mesh as been adjusted to match a print surface. In order to do + * this the Unified Bed Leveling System is turned on by the G26 command. The D parameter + * alters the command's normal behavior and disables the Unified Bed Leveling System even if + * it is on. + * + * H # Hotend Set the Nozzle Temperature. If not specified, a default of 205 C. will be assumed. + * + * I # Preset Heat the Nozzle and Bed based on a Material Preset (if material presets are defined). + * + * F # Filament Used to specify the diameter of the filament being used. If not specified + * 1.75mm filament is assumed. If you are not getting acceptable results by using the + * 'correct' numbers, you can scale this number up or down a little bit to change the amount + * of filament that is being extruded during the printing of the various lines on the bed. + * + * K Keep-On Keep the heaters turned on at the end of the command. + * + * L # Layer Layer height. (Height of nozzle above bed) If not specified .20mm will be used. + * + * O # Ooooze How much your nozzle will Ooooze filament while getting in position to print. This + * is over kill, but using this parameter will let you get the very first 'circle' perfect + * so you have a trophy to peel off of the bed and hang up to show how perfectly you have your + * Mesh calibrated. If not specified, a filament length of .3mm is assumed. + * + * P # Prime Prime the nozzle with specified length of filament. If this parameter is not + * given, no prime action will take place. If the parameter specifies an amount, that much + * will be purged before continuing. If no amount is specified the command will start + * purging filament until the user provides an LCD Click and then it will continue with + * printing the Mesh. You can carefully remove the spent filament with a needle nose + * pliers while holding the LCD Click wheel in a depressed state. If you do not have + * an LCD, you must specify a value if you use P. + * + * Q # Multiplier Retraction Multiplier. (Normally not needed.) During G26 retraction will use the length + * specified by this parameter (1mm by default). Recover will be 1.2x the retract distance. + * + * R # Repeat Prints the number of patterns given as a parameter, starting at the current location. + * If a parameter isn't given, every point will be printed unless G26 is interrupted. + * This works the same way that the UBL G29 P4 R parameter works. + * + * NOTE: If you do not have an LCD, you -must- specify R. This is to ensure that you are + * aware that there's some risk associated with printing without the ability to abort in + * cases where mesh point Z value may be inaccurate. As above, if you do not include a + * parameter, every point will be printed. + * + * S # Nozzle Used to control the size of nozzle diameter. If not specified, a .4mm nozzle is assumed. + * + * U # Random Randomize the order that the circles are drawn on the bed. The search for the closest + * un-drawn circle is still done. But the distance to the location for each circle has a + * random number of the specified size added to it. Specifying S50 will give an interesting + * deviation from the normal behavior on a 10 x 10 Mesh. + * + * X # X Coord. Specify the starting location of the drawing activity. + * + * Y # Y Coord. Specify the starting location of the drawing activity. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(G26_MESH_VALIDATION) + +#define G26_OK false +#define G26_ERR true + +#include "../../gcode/gcode.h" +#include "../../feature/bedlevel/bedlevel.h" + +#include "../../MarlinCore.h" +#include "../../module/planner.h" +#include "../../module/motion.h" +#include "../../module/tool_change.h" +#include "../../module/temperature.h" +#include "../../lcd/marlinui.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../lcd/extui/ui_api.h" +#endif + +#if ENABLED(UBL_HILBERT_CURVE) + #include "../../feature/bedlevel/hilbert_curve.h" +#endif + +#define EXTRUSION_MULTIPLIER 1.0 +#define PRIME_LENGTH 10.0 +#define OOZE_AMOUNT 0.3 + +#define INTERSECTION_CIRCLE_RADIUS 5 +#define CROSSHAIRS_SIZE 3 + +#ifndef G26_RETRACT_MULTIPLIER + #define G26_RETRACT_MULTIPLIER 1.0 // x 1mm +#endif + +#ifndef G26_XY_FEEDRATE + #define G26_XY_FEEDRATE (PLANNER_XY_FEEDRATE() / 3.0) +#endif + +#ifndef G26_XY_FEEDRATE_TRAVEL + #define G26_XY_FEEDRATE_TRAVEL (PLANNER_XY_FEEDRATE() / 1.5) +#endif + +#if CROSSHAIRS_SIZE >= INTERSECTION_CIRCLE_RADIUS + #error "CROSSHAIRS_SIZE must be less than INTERSECTION_CIRCLE_RADIUS." +#endif + +#define G26_OK false +#define G26_ERR true + +#if ENABLED(ARC_SUPPORT) + void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t); +#endif + +constexpr float g26_e_axis_feedrate = 0.025; + +static MeshFlags circle_flags; +float g26_random_deviation = 0.0; + +#if HAS_MARLINUI_MENU + + /** + * If the LCD is clicked, cancel, wait for release, return true + */ + bool user_canceled() { + if (!ui.button_pressed()) return false; // Return if the button isn't pressed + ui.set_status(GET_TEXT_F(MSG_G26_CANCELED), 99); + TERN_(HAS_MARLINUI_MENU, ui.quick_feedback()); + ui.wait_for_release(); + return true; + } + +#endif + +void move_to(const_float_t rx, const_float_t ry, const_float_t z, const_float_t e_delta) { + static float last_z = -999.99; + + const xy_pos_t dest = { rx, ry }; + + const bool has_xy_component = dest != current_position, // Check if X or Y is involved in the movement. + has_e_component = e_delta != 0.0; + + if (z != last_z) { + last_z = z; + destination.set(current_position.x, current_position.y, z, current_position.e); + const feedRate_t fr_mm_s = planner.settings.max_feedrate_mm_s[Z_AXIS] * 0.5f; // Use half of the Z_AXIS max feed rate + prepare_internal_move_to_destination(fr_mm_s); + } + + // If X or Y in combination with E is involved do a 'normal' move. + // If X or Y with no E is involved do a 'fast' move + // Otherwise retract/recover/hop. + destination = dest; + destination.e += e_delta; + const feedRate_t fr_mm_s = has_xy_component + ? (has_e_component ? feedRate_t(G26_XY_FEEDRATE) : feedRate_t(G26_XY_FEEDRATE_TRAVEL)) + : planner.settings.max_feedrate_mm_s[E_AXIS] * 0.666f; + prepare_internal_move_to_destination(fr_mm_s); +} + +void move_to(const xyz_pos_t &where, const_float_t de) { move_to(where.x, where.y, where.z, de); } + +typedef struct { + float extrusion_multiplier = EXTRUSION_MULTIPLIER, + retraction_multiplier = G26_RETRACT_MULTIPLIER, + layer_height = MESH_TEST_LAYER_HEIGHT, + prime_length = PRIME_LENGTH; + + celsius_t bed_temp = MESH_TEST_BED_TEMP, + hotend_temp = MESH_TEST_HOTEND_TEMP; + + float nozzle = MESH_TEST_NOZZLE_SIZE, + filament_diameter = DEFAULT_NOMINAL_FILAMENT_DIA, + ooze_amount; // 'O' ... OOZE_AMOUNT + + bool continue_with_closest, // 'C' + keep_heaters_on; // 'K' + + xy_pos_t xy_pos; // = { 0, 0 } + + int8_t prime_flag = 0; + + bool g26_retracted = false; // Track the retracted state during G26 so mismatched + // retracts/recovers don't result in a bad state. + + void retract_filament(const xyz_pos_t &where) { + if (!g26_retracted) { // Only retract if we are not already retracted! + g26_retracted = true; + move_to(where, -1.0f * retraction_multiplier); + } + } + + // TODO: Parameterize the Z lift with a define + void retract_lift_move(const xyz_pos_t &s) { + retract_filament(destination); + move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0f); // Z lift to minimize scraping + move_to(s.x, s.y, s.z + 0.5f, 0.0f); // Get to the starting point with no extrusion while lifted + } + + void recover_filament(const xyz_pos_t &where) { + if (g26_retracted) { // Only un-retract if we are retracted. + move_to(where, 1.2f * retraction_multiplier); + g26_retracted = false; + } + } + + /** + * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one + * to the other. But there are really three sets of coordinates involved. The first coordinate + * is the present location of the nozzle. We don't necessarily want to print from this location. + * We first need to move the nozzle to the start of line segment where we want to print. Once + * there, we can use the two coordinates supplied to draw the line. + * + * Note: Although we assume the first set of coordinates is the start of the line and the second + * set of coordinates is the end of the line, it does not always work out that way. This function + * optimizes the movement to minimize the travel distance before it can start printing. This saves + * a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does + * cause a lot of very little short retracement of th nozzle when it draws the very first line + * segment of a 'circle'. The time this requires is very short and is easily saved by the other + * cases where the optimization comes into play. + */ + void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) { + + // Distances to the start / end of the line + xy_float_t svec = current_position - s, evec = current_position - e; + + const float dist_start = HYPOT2(svec.x, svec.y), + dist_end = HYPOT2(evec.x, evec.y), + line_length = HYPOT(e.x - s.x, e.y - s.y); + + // If the end point of the line is closer to the nozzle, flip the direction, + // moving from the end to the start. On very small lines the optimization isn't worth it. + if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length)) + return print_line_from_here_to_there(e, s); + + // Decide whether to retract & lift + if (dist_start > 2.0) retract_lift_move(s); + + move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift + + const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier; + + recover_filament(destination); + move_to(e, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion + } + + void connect_neighbor_with_line(const xy_int8_t &p1, int8_t dx, int8_t dy) { + xy_int8_t p2; + p2.x = p1.x + dx; + p2.y = p1.y + dy; + + if (p2.x < 0 || p2.x >= (GRID_MAX_POINTS_X)) return; + if (p2.y < 0 || p2.y >= (GRID_MAX_POINTS_Y)) return; + + if (circle_flags.marked(p1.x, p1.y) && circle_flags.marked(p2.x, p2.y)) { + xyz_pos_t s, e; + s.x = bedlevel.get_mesh_x(p1.x) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx; + e.x = bedlevel.get_mesh_x(p2.x) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx; + s.y = bedlevel.get_mesh_y(p1.y) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy; + e.y = bedlevel.get_mesh_y(p2.y) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy; + s.z = e.z = layer_height; + + #if HAS_ENDSTOPS + LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1); + LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1); + #endif + + if (position_is_reachable(s) && position_is_reachable(e)) + print_line_from_here_to_there(s, e); + } + } + + /** + * Turn on the bed and nozzle heat and + * wait for them to get up to temperature. + */ + bool turn_on_heaters() { + + SERIAL_ECHOLNPGM("Waiting for heatup."); + + #if HAS_HEATED_BED + + if (bed_temp > 25) { + #if HAS_WIRED_LCD + ui.set_status(GET_TEXT_F(MSG_G26_HEATING_BED), 99); + ui.quick_feedback(); + TERN_(HAS_MARLINUI_MENU, ui.capture()); + #endif + thermalManager.setTargetBed(bed_temp); + + // Wait for the temperature to stabilize + if (!thermalManager.wait_for_bed(true OPTARG(G26_CLICK_CAN_CANCEL, true))) + return G26_ERR; + } + + #else + + UNUSED(bed_temp); + + #endif // HAS_HEATED_BED + + // Start heating the active nozzle + #if HAS_WIRED_LCD + ui.set_status(GET_TEXT_F(MSG_G26_HEATING_NOZZLE), 99); + ui.quick_feedback(); + #endif + thermalManager.setTargetHotend(hotend_temp, active_extruder); + + // Wait for the temperature to stabilize + if (!thermalManager.wait_for_hotend(active_extruder, true OPTARG(G26_CLICK_CAN_CANCEL, true))) + return G26_ERR; + + #if HAS_WIRED_LCD + ui.reset_status(); + ui.quick_feedback(); + #endif + + return G26_OK; + } + + /** + * Prime the nozzle if needed. Return true on error. + */ + bool prime_nozzle() { + + const feedRate_t fr_slow_e = planner.settings.max_feedrate_mm_s[E_AXIS] / 15.0f; + #if HAS_MARLINUI_MENU && !HAS_TOUCH_BUTTONS // ui.button_pressed issue with touchscreen + #if ENABLED(PREVENT_LENGTHY_EXTRUDE) + float Total_Prime = 0.0; + #endif + + if (prime_flag == -1) { // The user wants to control how much filament gets purged + ui.capture(); + ui.set_status(GET_TEXT_F(MSG_G26_MANUAL_PRIME), 99); + ui.chirp(); + + destination = current_position; + + recover_filament(destination); // Make sure G26 doesn't think the filament is retracted(). + + while (!ui.button_pressed()) { + ui.chirp(); + destination.e += 0.25; + #if ENABLED(PREVENT_LENGTHY_EXTRUDE) + Total_Prime += 0.25; + if (Total_Prime >= EXTRUDE_MAXLENGTH) { + ui.release(); + return G26_ERR; + } + #endif + prepare_internal_move_to_destination(fr_slow_e); + destination = current_position; + planner.synchronize(); // Without this synchronize, the purge is more consistent, + // but because the planner has a buffer, we won't be able + // to stop as quickly. So we put up with the less smooth + // action to give the user a more responsive 'Stop'. + } + + ui.wait_for_release(); + + ui.set_status(GET_TEXT_F(MSG_G26_PRIME_DONE), 99); + ui.quick_feedback(); + ui.release(); + } + else + #endif + { + #if HAS_WIRED_LCD + ui.set_status(GET_TEXT_F(MSG_G26_FIXED_LENGTH), 99); + ui.quick_feedback(); + #endif + destination = current_position; + destination.e += prime_length; + prepare_internal_move_to_destination(fr_slow_e); + destination.e -= prime_length; + retract_filament(destination); + } + + return G26_OK; + } + + /** + * Find the nearest point at which to print a circle + */ + mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) { + + mesh_index_pair out_point; + out_point.pos = -1; + + #if ENABLED(UBL_HILBERT_CURVE) + + auto test_func = [](uint8_t i, uint8_t j, void *data) -> bool { + if (!circle_flags.marked(i, j)) { + mesh_index_pair *out_point = (mesh_index_pair*)data; + out_point->pos.set(i, j); // Save its data + return true; + } + return false; + }; + + hilbert_curve::search_from_closest(pos, test_func, &out_point); + + #else + + float closest = 99999.99; + + GRID_LOOP(i, j) { + if (!circle_flags.marked(i, j)) { + // We found a circle that needs to be printed + const xy_pos_t m = { bedlevel.get_mesh_x(i), bedlevel.get_mesh_y(j) }; + + // Get the distance to this intersection + float f = (pos - m).magnitude(); + + // It is possible that we are being called with the values + // to let us find the closest circle to the start position. + // But if this is not the case, add a small weighting to the + // distance calculation to help it choose a better place to continue. + f += (xy_pos - m).magnitude() / 15.0f; + + // Add the specified amount of Random Noise to our search + if (g26_random_deviation > 1.0) f += random(0.0, g26_random_deviation); + + if (f < closest) { + closest = f; // Found a closer un-printed location + out_point.pos.set(i, j); // Save its data + out_point.distance = closest; + } + } + } + + #endif + + circle_flags.mark(out_point); // Mark this location as done. + return out_point; + } + +} g26_helper_t; + +/** + * G26: Mesh Validation Pattern generation. + * + * Used to interactively edit the mesh by placing the + * nozzle in a problem area and doing a G29 P4 R command. + * + * Parameters: + * + * B Bed Temperature + * C Continue from the Closest mesh point + * D Disable leveling before starting + * F Filament diameter + * H Hotend Temperature + * K Keep heaters on when completed + * L Layer Height + * O Ooze extrusion length + * P Prime length + * Q Retraction multiplier + * R Repetitions (number of grid points) + * S Nozzle Size (diameter) in mm + * T Tool index to change to, if included + * U Random deviation (50 if no value given) + * X X position + * Y Y position + */ +void GcodeSuite::G26() { + SERIAL_ECHOLNPGM("G26 starting..."); + + // Don't allow Mesh Validation without homing first, + // or if the parameter parsing did not go OK, abort + if (homing_needed_error()) return; + + // Change the tool first, if specified + if (parser.seenval('T')) tool_change(parser.value_int()); + + g26_helper_t g26; + + g26.ooze_amount = parser.linearval('O', OOZE_AMOUNT); + g26.continue_with_closest = parser.boolval('C'); + g26.keep_heaters_on = parser.boolval('K'); + + // Accept 'I' if temperature presets are defined + #if HAS_PREHEAT + const uint8_t preset_index = parser.seenval('I') ? _MIN(parser.value_byte(), PREHEAT_COUNT - 1) + 1 : 0; + #endif + + #if HAS_HEATED_BED + + // Get a temperature from 'I' or 'B' + celsius_t bedtemp = 0; + + // Use the 'I' index if temperature presets are defined + #if HAS_PREHEAT + if (preset_index) bedtemp = ui.material_preset[preset_index - 1].bed_temp; + #endif + + // Look for 'B' Bed Temperature + if (parser.seenval('B')) bedtemp = parser.value_celsius(); + + if (bedtemp) { + if (!WITHIN(bedtemp, 40, BED_MAX_TARGET)) { + SERIAL_ECHOLNPGM("?Specified bed temperature not plausible (40-", BED_MAX_TARGET, "C)."); + return; + } + g26.bed_temp = bedtemp; + } + + #endif // HAS_HEATED_BED + + if (parser.seenval('L')) { + g26.layer_height = parser.value_linear_units(); + if (!WITHIN(g26.layer_height, 0.0, 2.0)) { + SERIAL_ECHOLNPGM("?Specified layer height not plausible."); + return; + } + } + + if (parser.seen('Q')) { + if (parser.has_value()) { + g26.retraction_multiplier = parser.value_float(); + if (!WITHIN(g26.retraction_multiplier, 0.05, 15.0)) { + SERIAL_ECHOLNPGM("?Specified Retraction Multiplier not plausible."); + return; + } + } + else { + SERIAL_ECHOLNPGM("?Retraction Multiplier must be specified."); + return; + } + } + + if (parser.seenval('S')) { + g26.nozzle = parser.value_float(); + if (!WITHIN(g26.nozzle, 0.1, 2.0)) { + SERIAL_ECHOLNPGM("?Specified nozzle size not plausible."); + return; + } + } + + if (parser.seen('P')) { + if (!parser.has_value()) { + #if HAS_MARLINUI_MENU + g26.prime_flag = -1; + #else + SERIAL_ECHOLNPGM("?Prime length must be specified when not using an LCD."); + return; + #endif + } + else { + g26.prime_flag++; + g26.prime_length = parser.value_linear_units(); + if (!WITHIN(g26.prime_length, 0.0, 25.0)) { + SERIAL_ECHOLNPGM("?Specified prime length not plausible."); + return; + } + } + } + + if (parser.seenval('F')) { + g26.filament_diameter = parser.value_linear_units(); + if (!WITHIN(g26.filament_diameter, 1.0, 4.0)) { + SERIAL_ECHOLNPGM("?Specified filament size not plausible."); + return; + } + } + g26.extrusion_multiplier *= sq(1.75) / sq(g26.filament_diameter); // If we aren't using 1.75mm filament, we need to + // scale up or down the length needed to get the + // same volume of filament + + g26.extrusion_multiplier *= g26.filament_diameter * sq(g26.nozzle) / sq(0.3); // Scale up by nozzle size + + // Get a temperature from 'I' or 'H' + celsius_t noztemp = 0; + + // Accept 'I' if temperature presets are defined + #if HAS_PREHEAT + if (preset_index) noztemp = ui.material_preset[preset_index - 1].hotend_temp; + #endif + + // Look for 'H' Hotend Temperature + if (parser.seenval('H')) noztemp = parser.value_celsius(); + + // If any preset or temperature was specified + if (noztemp) { + if (!WITHIN(noztemp, 165, (HEATER_0_MAXTEMP) - (HOTEND_OVERSHOOT))) { + SERIAL_ECHOLNPGM("?Specified nozzle temperature not plausible."); + return; + } + g26.hotend_temp = noztemp; + } + + // 'U' to Randomize and optionally set circle deviation + if (parser.seen('U')) { + randomSeed(millis()); + // This setting will persist for the next G26 + g26_random_deviation = parser.has_value() ? parser.value_float() : 50.0; + } + + // Get repeat from 'R', otherwise do one full circuit + int16_t g26_repeats; + #if HAS_MARLINUI_MENU + g26_repeats = parser.intval('R', GRID_MAX_POINTS + 1); + #else + if (parser.seen('R')) + g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1; + else { + SERIAL_ECHOLNPGM("?(R)epeat must be specified when not using an LCD."); + return; + } + #endif + if (g26_repeats < 1) { + SERIAL_ECHOLNPGM("?(R)epeat value not plausible; must be at least 1."); + return; + } + + // Set a position with 'X' and/or 'Y'. Default: current_position + g26.xy_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x, + parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y); + if (!position_is_reachable(g26.xy_pos)) { + SERIAL_ECHOLNPGM("?Specified X,Y coordinate out of bounds."); + return; + } + + /** + * Wait until all parameters are verified before altering the state! + */ + set_bed_leveling_enabled(!parser.seen_test('D')); + + do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES); + + #if DISABLED(NO_VOLUMETRICS) + bool volumetric_was_enabled = parser.volumetric_enabled; + parser.volumetric_enabled = false; + planner.calculate_volumetric_multipliers(); + #endif + + if (g26.turn_on_heaters() != G26_OK) goto LEAVE; + + current_position.e = 0.0; + sync_plan_position_e(); + + if (g26.prime_flag && g26.prime_nozzle() != G26_OK) goto LEAVE; + + /** + * Bed is preheated + * + * Nozzle is at temperature + * + * Filament is primed! + * + * It's "Show Time" !!! + */ + + circle_flags.reset(); + + // Move nozzle to the specified height for the first layer + destination = current_position; + destination.z = g26.layer_height; + move_to(destination, 0.0); + move_to(destination, g26.ooze_amount); + + TERN_(HAS_MARLINUI_MENU, ui.capture()); + + #if DISABLED(ARC_SUPPORT) + + /** + * Pre-generate radius offset values at 30 degree intervals to reduce CPU load. + */ + #define A_INT 30 + #define _ANGS (360 / A_INT) + #define A_CNT (_ANGS / 2) + #define _IND(A) ((A + _ANGS * 8) % _ANGS) + #define _COS(A) (trig_table[_IND(A) % A_CNT] * (_IND(A) >= A_CNT ? -1 : 1)) + #define _SIN(A) (-_COS((A + A_CNT / 2) % _ANGS)) + #if A_CNT & 1 + #error "A_CNT must be a positive value. Please change A_INT." + #endif + float trig_table[A_CNT]; + LOOP_L_N(i, A_CNT) + trig_table[i] = INTERSECTION_CIRCLE_RADIUS * cos(RADIANS(i * A_INT)); + + #endif // !ARC_SUPPORT + + mesh_index_pair location; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_START)); + do { + // Find the nearest confluence + location = g26.find_closest_circle_to_print(g26.continue_with_closest ? xy_pos_t(current_position) : g26.xy_pos); + + if (location.valid()) { + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_START)); + const xy_pos_t circle = { bedlevel.get_mesh_x(location.pos.a), bedlevel.get_mesh_y(location.pos.b) }; + + // If this mesh location is outside the printable radius, skip it. + if (!position_is_reachable(circle)) continue; + + // Determine where to start and end the circle, + // which is always drawn counter-clockwise. + const xy_int8_t st = location; + const bool f = st.y == 0, + r = st.x >= (GRID_MAX_POINTS_X) - 1, + b = st.y >= (GRID_MAX_POINTS_Y) - 1; + + #if ENABLED(ARC_SUPPORT) + + #define ARC_LENGTH(quarters) (INTERSECTION_CIRCLE_RADIUS * M_PI * (quarters) / 2) + #define INTERSECTION_CIRCLE_DIAM ((INTERSECTION_CIRCLE_RADIUS) * 2) + + xy_float_t e = { circle.x + INTERSECTION_CIRCLE_RADIUS, circle.y }; + xyz_float_t s = e; + + // Figure out where to start and end the arc - we always print counterclockwise + float arc_length = ARC_LENGTH(4); + if (st.x == 0) { // left edge + if (!f) { s.x = circle.x; s.y -= INTERSECTION_CIRCLE_RADIUS; } + if (!b) { e.x = circle.x; e.y += INTERSECTION_CIRCLE_RADIUS; } + arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2); + } + else if (r) { // right edge + if (b) s.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y); + else s.set(circle.x, circle.y + INTERSECTION_CIRCLE_RADIUS); + if (f) e.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y); + else e.set(circle.x, circle.y - (INTERSECTION_CIRCLE_RADIUS)); + arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2); + } + else if (f) { + e.x -= INTERSECTION_CIRCLE_DIAM; + arc_length = ARC_LENGTH(2); + } + else if (b) { + s.x -= INTERSECTION_CIRCLE_DIAM; + arc_length = ARC_LENGTH(2); + } + + const ab_float_t arc_offset = circle - s; + const xy_float_t dist = current_position - s; // Distance from the start of the actual circle + const float dist_start = HYPOT2(dist.x, dist.y); + const xyze_pos_t endpoint = { + e.x, e.y, g26.layer_height, + current_position.e + (arc_length * g26_e_axis_feedrate * g26.extrusion_multiplier) + }; + + if (dist_start > 2.0) { + s.z = g26.layer_height + 0.5f; + g26.retract_lift_move(s); + } + + s.z = g26.layer_height; + move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift + + g26.recover_filament(destination); + + { REMEMBER(fr, feedrate_mm_s, PLANNER_XY_FEEDRATE() * 0.1f); + plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc + destination = current_position; + } + + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation + + #else // !ARC_SUPPORT + + int8_t start_ind = -2, end_ind = 9; // Assume a full circle (from 5:00 to 5:00) + if (st.x == 0) { // Left edge? Just right half. + start_ind = f ? 0 : -3; // 03:00 to 12:00 for front-left + end_ind = b ? 0 : 2; // 06:00 to 03:00 for back-left + } + else if (r) { // Right edge? Just left half. + start_ind = b ? 6 : 3; // 12:00 to 09:00 for front-right + end_ind = f ? 5 : 8; // 09:00 to 06:00 for back-right + } + else if (f) { // Front edge? Just back half. + start_ind = 0; // 03:00 + end_ind = 5; // 09:00 + } + else if (b) { // Back edge? Just front half. + start_ind = 6; // 09:00 + end_ind = 11; // 03:00 + } + + for (int8_t ind = start_ind; ind <= end_ind; ind++) { + + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation + + xyz_float_t p = { circle.x + _COS(ind ), circle.y + _SIN(ind ), g26.layer_height }, + q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26.layer_height }; + + #if IS_KINEMATIC + // Check to make sure this segment is entirely on the bed, skip if not. + if (!position_is_reachable(p) || !position_is_reachable(q)) continue; + #elif HAS_ENDSTOPS + LIMIT(p.x, X_MIN_POS + 1, X_MAX_POS - 1); // Prevent hitting the endstops + LIMIT(p.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + LIMIT(q.x, X_MIN_POS + 1, X_MAX_POS - 1); + LIMIT(q.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + #endif + + g26.print_line_from_here_to_there(p, q); + SERIAL_FLUSH(); // Prevent host M105 buffer overrun. + } + + #endif // !ARC_SUPPORT + + g26.connect_neighbor_with_line(location.pos, -1, 0); + g26.connect_neighbor_with_line(location.pos, 1, 0); + g26.connect_neighbor_with_line(location.pos, 0, -1); + g26.connect_neighbor_with_line(location.pos, 0, 1); + planner.synchronize(); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_FINISH)); + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; + } + + SERIAL_FLUSH(); // Prevent host M105 buffer overrun. + + } while (--g26_repeats && location.valid()); + + LEAVE: + ui.set_status(GET_TEXT_F(MSG_G26_LEAVING), -1); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, ExtUI::G26_FINISH)); + + g26.retract_filament(destination); + destination.z = Z_CLEARANCE_BETWEEN_PROBES; + move_to(destination, 0); // Raise the nozzle + + #if DISABLED(NO_VOLUMETRICS) + parser.volumetric_enabled = volumetric_was_enabled; + planner.calculate_volumetric_multipliers(); + #endif + + TERN_(HAS_MARLINUI_MENU, ui.release()); // Give back control of the LCD + + if (!g26.keep_heaters_on) { + TERN_(HAS_HEATED_BED, thermalManager.setTargetBed(0)); + thermalManager.setTargetHotend(active_extruder, 0); + } +} + +#endif // G26_MESH_VALIDATION diff --git a/src/gcode/bedlevel/G35.cpp b/src/gcode/bedlevel/G35.cpp new file mode 100644 index 0000000..dd828bf --- /dev/null +++ b/src/gcode/bedlevel/G35.cpp @@ -0,0 +1,175 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(ASSISTED_TRAMMING) + +#include "../gcode.h" +#include "../../module/planner.h" +#include "../../module/probe.h" +#include "../../feature/bedlevel/bedlevel.h" + +#if HAS_MULTI_HOTEND + #include "../../module/tool_change.h" +#endif + +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +// +// Define tramming point names. +// + +#include "../../feature/tramming.h" + +/** + * G35: Read bed corners to help adjust bed screws + * + * S + * + * Screw thread: 30 - Clockwise M3 + * 31 - Counter-Clockwise M3 + * 40 - Clockwise M4 + * 41 - Counter-Clockwise M4 + * 50 - Clockwise M5 + * 51 - Counter-Clockwise M5 + **/ +void GcodeSuite::G35() { + DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING)); + + if (DEBUGGING(LEVELING)) log_machine_info(); + + float z_measured[G35_PROBE_COUNT] = { 0 }; + + const uint8_t screw_thread = parser.byteval('S', TRAMMING_SCREW_THREAD); + if (!WITHIN(screw_thread, 30, 51) || screw_thread % 10 > 1) { + SERIAL_ECHOLNPGM("?(S)crew thread must be 30, 31, 40, 41, 50, or 51."); + return; + } + + // Wait for planner moves to finish! + planner.synchronize(); + + // Disable the leveling matrix before auto-aligning + #if HAS_LEVELING + #if ENABLED(RESTORE_LEVELING_AFTER_G35) + const bool leveling_was_active = planner.leveling_active; + #endif + set_bed_leveling_enabled(false); + #endif + + #if ENABLED(CNC_WORKSPACE_PLANES) + workspace_plane = PLANE_XY; + #endif + + // Always home with tool 0 active + #if HAS_MULTI_HOTEND + const uint8_t old_tool_index = active_extruder; + tool_change(0, true); + #endif + + // Disable duplication mode on homing + TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false)); + + // Home only Z axis when X and Y is trusted, otherwise all axes, if needed before this procedure + if (!all_axes_trusted()) process_subcommands_now(F("G28Z")); + + bool err_break = false; + + // Probe all positions + LOOP_L_N(i, G35_PROBE_COUNT) { + + // In BLTOUCH HS mode, the probe travels in a deployed state. + // Users of G35 might have a badly misaligned bed, so raise Z by the + // length of the deployed pin (BLTOUCH stroke < 7mm) + + // Unsure if this is even required. The probe seems to lift correctly after probe done. + do_blocking_move_to_z(SUM_TERN(BLTOUCH, Z_CLEARANCE_BETWEEN_PROBES, bltouch.z_extra_clearance())); + const float z_probed_height = probe.probe_at_point(tramming_points[i], PROBE_PT_RAISE, 0, true); + + if (isnan(z_probed_height)) { + SERIAL_ECHOPGM("G35 failed at point ", i + 1, " ("); + SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i])); + SERIAL_CHAR(')'); + SERIAL_ECHOLNPGM_P(SP_X_STR, tramming_points[i].x, SP_Y_STR, tramming_points[i].y); + err_break = true; + break; + } + + if (DEBUGGING(LEVELING)) { + DEBUG_ECHOPGM("Probing point ", i + 1, " ("); + DEBUG_ECHOF(FPSTR(pgm_read_ptr(&tramming_point_name[i]))); + DEBUG_CHAR(')'); + DEBUG_ECHOLNPGM_P(SP_X_STR, tramming_points[i].x, SP_Y_STR, tramming_points[i].y, SP_Z_STR, z_probed_height); + } + + z_measured[i] = z_probed_height; + } + + if (!err_break) { + const float threads_factor[] = { 0.5, 0.7, 0.8 }; + + // Calculate adjusts + LOOP_S_L_N(i, 1, G35_PROBE_COUNT) { + const float diff = z_measured[0] - z_measured[i], + adjust = ABS(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10]; + + const int full_turns = trunc(adjust); + const float decimal_part = adjust - float(full_turns); + const int minutes = trunc(decimal_part * 60.0f); + + SERIAL_ECHOPGM("Turn "); + SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i])); + SERIAL_ECHOPGM(" ", (screw_thread & 1) == (adjust > 0) ? "CCW" : "CW", " by ", ABS(full_turns), " turns"); + if (minutes) SERIAL_ECHOPGM(" and ", ABS(minutes), " minutes"); + if (ENABLED(REPORT_TRAMMING_MM)) SERIAL_ECHOPGM(" (", -diff, "mm)"); + SERIAL_EOL(); + } + } + else + SERIAL_ECHOLNPGM("G35 aborted."); + + // Restore the active tool after homing + #if HAS_MULTI_HOTEND + if (old_tool_index != 0) tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER + #endif + + #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35) + set_bed_leveling_enabled(leveling_was_active); + #endif + + // Stow the probe, as the last call to probe.probe_at_point(...) left + // the probe deployed if it was successful. + probe.stow(); + + move_to_tramming_wait_pos(); + + // After this operation the Z position needs correction + set_axis_never_homed(Z_AXIS); +} + +#endif // ASSISTED_TRAMMING diff --git a/src/gcode/bedlevel/G42.cpp b/src/gcode/bedlevel/G42.cpp new file mode 100644 index 0000000..cb5ed97 --- /dev/null +++ b/src/gcode/bedlevel/G42.cpp @@ -0,0 +1,73 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_MESH + +#include "../gcode.h" +#include "../../MarlinCore.h" // for IsRunning() +#include "../../module/motion.h" +#include "../../module/probe.h" // for probe.offset +#include "../../feature/bedlevel/bedlevel.h" + +/** + * G42: Move X & Y axes to mesh coordinates (I & J) + */ +void GcodeSuite::G42() { + if (MOTION_CONDITIONS) { + const bool hasI = parser.seenval('I'); + const int8_t ix = hasI ? parser.value_int() : 0; + const bool hasJ = parser.seenval('J'); + const int8_t iy = hasJ ? parser.value_int() : 0; + + if ((hasI && !WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) || (hasJ && !WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1))) { + SERIAL_ECHOLNPGM(STR_ERR_MESH_XY); + return; + } + + // Move to current_position, as modified by I, J, P parameters + destination = current_position; + + if (hasI) destination.x = bedlevel.get_mesh_x(ix); + if (hasJ) destination.y = bedlevel.get_mesh_y(iy); + + #if HAS_PROBE_XY_OFFSET + if (parser.boolval('P')) { + if (hasI) destination.x -= probe.offset_xy.x; + if (hasJ) destination.y -= probe.offset_xy.y; + } + #endif + + const feedRate_t fval = parser.linearval('F'), + fr_mm_s = MMM_TO_MMS(fval > 0 ? fval : 0.0f); + + // SCARA kinematic has "safe" XY raw moves + #if IS_SCARA + prepare_internal_fast_move_to_destination(fr_mm_s); + #else + prepare_internal_move_to_destination(fr_mm_s); + #endif + } +} + +#endif // HAS_MESH diff --git a/src/gcode/bedlevel/M420.cpp b/src/gcode/bedlevel/M420.cpp new file mode 100644 index 0000000..277f95b --- /dev/null +++ b/src/gcode/bedlevel/M420.cpp @@ -0,0 +1,261 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_LEVELING + +#include "../gcode.h" +#include "../../feature/bedlevel/bedlevel.h" +#include "../../module/planner.h" +#include "../../module/probe.h" + +#if ENABLED(EEPROM_SETTINGS) + #include "../../module/settings.h" +#endif + +#if ENABLED(EXTENSIBLE_UI) + #include "../../lcd/extui/ui_api.h" +#endif + +//#define M420_C_USE_MEAN + +/** + * M420: Enable/Disable Bed Leveling and/or set the Z fade height. + * + * S[bool] Turns leveling on or off + * Z[height] Sets the Z fade height (0 or none to disable) + * V[bool] Verbose - Print the leveling grid + * + * With AUTO_BED_LEVELING_UBL only: + * + * L[index] Load UBL mesh from index (0 is default) + * T[map] 0:Human-readable 1:CSV 2:"LCD" 4:Compact + * + * With mesh-based leveling only: + * + * C Center mesh on the mean of the lowest and highest + * + * With MARLIN_DEV_MODE: + * S2 Create a simple random mesh and enable + */ +void GcodeSuite::M420() { + const bool seen_S = parser.seen('S'), + to_enable = seen_S ? parser.value_bool() : planner.leveling_active; + + #if ENABLED(MARLIN_DEV_MODE) + if (parser.intval('S') == 2) { + const float x_min = probe.min_x(), x_max = probe.max_x(), + y_min = probe.min_y(), y_max = probe.max_y(); + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + xy_pos_t start, spacing; + start.set(x_min, y_min); + spacing.set((x_max - x_min) / (GRID_MAX_CELLS_X), + (y_max - y_min) / (GRID_MAX_CELLS_Y)); + bedlevel.set_grid(spacing, start); + #endif + GRID_LOOP(x, y) { + bedlevel.z_values[x][y] = 0.001 * random(-200, 200); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, bedlevel.z_values[x][y])); + } + TERN_(AUTO_BED_LEVELING_BILINEAR, bedlevel.refresh_bed_level()); + SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh "); + SERIAL_ECHOPGM(" (", x_min); + SERIAL_CHAR(','); SERIAL_ECHO(y_min); + SERIAL_ECHOPGM(")-(", x_max); + SERIAL_CHAR(','); SERIAL_ECHO(y_max); + SERIAL_ECHOLNPGM(")"); + } + #endif + + xyz_pos_t oldpos = current_position; + + // If disabling leveling do it right away + // (Don't disable for just M420 or M420 V) + if (seen_S && !to_enable) set_bed_leveling_enabled(false); + + #if ENABLED(AUTO_BED_LEVELING_UBL) + + // L to load a mesh from the EEPROM + if (parser.seen('L')) { + + set_bed_leveling_enabled(false); + + #if ENABLED(EEPROM_SETTINGS) + const int8_t storage_slot = parser.has_value() ? parser.value_int() : bedlevel.storage_slot; + const int16_t a = settings.calc_num_meshes(); + + if (!a) { + SERIAL_ECHOLNPGM("?EEPROM storage not available."); + return; + } + + if (!WITHIN(storage_slot, 0, a - 1)) { + SERIAL_ECHOLNPGM("?Invalid storage slot."); + SERIAL_ECHOLNPGM("?Use 0 to ", a - 1); + return; + } + + settings.load_mesh(storage_slot); + bedlevel.storage_slot = storage_slot; + + #else + + SERIAL_ECHOLNPGM("?EEPROM storage not available."); + return; + + #endif + } + + // L or V display the map info + if (parser.seen("LV")) { + bedlevel.display_map(parser.byteval('T')); + SERIAL_ECHOPGM("Mesh is "); + if (!bedlevel.mesh_is_valid()) SERIAL_ECHOPGM("in"); + SERIAL_ECHOLNPGM("valid\nStorage slot: ", bedlevel.storage_slot); + } + + #endif // AUTO_BED_LEVELING_UBL + + const bool seenV = parser.seen_test('V'); + + #if HAS_MESH + + if (leveling_is_valid()) { + + // Subtract the given value or the mean from all mesh values + if (parser.seen('C')) { + const float cval = parser.value_float(); + #if ENABLED(AUTO_BED_LEVELING_UBL) + + set_bed_leveling_enabled(false); + bedlevel.adjust_mesh_to_mean(true, cval); + + #else + + #if ENABLED(M420_C_USE_MEAN) + + // Get the sum and average of all mesh values + float mesh_sum = 0; + GRID_LOOP(x, y) mesh_sum += bedlevel.z_values[x][y]; + const float zmean = mesh_sum / float(GRID_MAX_POINTS); + + #else // midrange + + // Find the low and high mesh values. + float lo_val = 100, hi_val = -100; + GRID_LOOP(x, y) { + const float z = bedlevel.z_values[x][y]; + NOMORE(lo_val, z); + NOLESS(hi_val, z); + } + // Get the midrange plus C value. (The median may be better.) + const float zmean = (lo_val + hi_val) / 2.0 + cval; + + #endif + + // If not very close to 0, adjust the mesh + if (!NEAR_ZERO(zmean)) { + set_bed_leveling_enabled(false); + // Subtract the mean from all values + GRID_LOOP(x, y) { + bedlevel.z_values[x][y] -= zmean; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, bedlevel.z_values[x][y])); + } + TERN_(AUTO_BED_LEVELING_BILINEAR, bedlevel.refresh_bed_level()); + } + + #endif + } + + } + else if (to_enable || seenV) { + SERIAL_ECHO_MSG("Invalid mesh."); + goto EXIT_M420; + } + + #endif // HAS_MESH + + // V to print the matrix or mesh + if (seenV) { + #if ABL_PLANAR + planner.bed_level_matrix.debug(F("Bed Level Correction Matrix:")); + #else + if (leveling_is_valid()) { + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + bedlevel.print_leveling_grid(); + #elif ENABLED(MESH_BED_LEVELING) + SERIAL_ECHOLNPGM("Mesh Bed Level data:"); + bedlevel.report_mesh(); + #endif + } + #endif + } + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false); + #endif + + // Enable leveling if specified, or if previously active + set_bed_leveling_enabled(to_enable); + + #if HAS_MESH + EXIT_M420: + #endif + + // Error if leveling failed to enable or reenable + if (to_enable && !planner.leveling_active) + SERIAL_ERROR_MSG(STR_ERR_M420_FAILED); + + SERIAL_ECHO_START(); + SERIAL_ECHOPGM("Bed Leveling "); + serialprintln_onoff(planner.leveling_active); + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + SERIAL_ECHO_START(); + SERIAL_ECHOPGM("Fade Height "); + if (planner.z_fade_height > 0.0) + SERIAL_ECHOLN(planner.z_fade_height); + else + SERIAL_ECHOLNPGM(STR_OFF); + #endif + + // Report change in position + if (oldpos != current_position) + report_current_position(); +} + +void GcodeSuite::M420_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F( + TERN(MESH_BED_LEVELING, "Mesh Bed Leveling", TERN(AUTO_BED_LEVELING_UBL, "Unified Bed Leveling", "Auto Bed Leveling")) + )); + SERIAL_ECHOF( + F(" M420 S"), planner.leveling_active + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + , FPSTR(SP_Z_STR), LINEAR_UNIT(planner.z_fade_height) + #endif + , F(" ; Leveling ") + ); + serialprintln_onoff(planner.leveling_active); +} + +#endif // HAS_LEVELING diff --git a/src/gcode/bedlevel/abl/G29.cpp b/src/gcode/bedlevel/abl/G29.cpp new file mode 100644 index 0000000..a540eae --- /dev/null +++ b/src/gcode/bedlevel/abl/G29.cpp @@ -0,0 +1,923 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * G29.cpp - Auto Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_ABL_NOT_UBL + +#include "../../gcode.h" +#include "../../../feature/bedlevel/bedlevel.h" +#include "../../../module/motion.h" +#include "../../../module/planner.h" +#include "../../../module/probe.h" +#include "../../queue.h" + +#if ENABLED(AUTO_BED_LEVELING_LINEAR) + #include "../../../libs/least_squares_fit.h" +#endif + +#if ABL_PLANAR + #include "../../../libs/vector_3.h" +#endif + +#include "../../../lcd/marlinui.h" +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD) + #include "../../../lcd/e3v2/creality/dwin.h" +#elif ENABLED(DWIN_LCD_PROUI) + #include "../../../lcd/e3v2/proui/dwin.h" +#endif + +#if HAS_MULTI_HOTEND + #include "../../../module/tool_change.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../../core/debug_out.h" + +#if ABL_USES_GRID + #if ENABLED(PROBE_Y_FIRST) + #define PR_OUTER_VAR abl.meshCount.x + #define PR_OUTER_SIZE abl.grid_points.x + #define PR_INNER_VAR abl.meshCount.y + #define PR_INNER_SIZE abl.grid_points.y + #else + #define PR_OUTER_VAR abl.meshCount.y + #define PR_OUTER_SIZE abl.grid_points.y + #define PR_INNER_VAR abl.meshCount.x + #define PR_INNER_SIZE abl.grid_points.x + #endif +#endif + +static void pre_g29_return(const bool retry, const bool did) { + if (!retry) { + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE, false)); + } + if (did) { + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_LevelingDone()); + TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone()); + } +} + +#define G29_RETURN(retry, did) do{ \ + pre_g29_return(TERN0(G29_RETRY_AND_RECOVER, retry), did); \ + return TERN_(G29_RETRY_AND_RECOVER, retry); \ +}while(0) + +// For manual probing values persist over multiple G29 +class G29_State { +public: + int verbose_level; + xy_pos_t probePos; + float measured_z; + bool dryrun, + reenable; + + #if HAS_MULTI_HOTEND + uint8_t tool_index; + #endif + + #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) + int abl_probe_index; + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + int abl_points; + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + static constexpr int abl_points = 3; + #elif ABL_USES_GRID + static constexpr int abl_points = GRID_MAX_POINTS; + #endif + + #if ABL_USES_GRID + + xy_int8_t meshCount; + + xy_pos_t probe_position_lf, + probe_position_rb; + + xy_float_t gridSpacing; // = { 0.0f, 0.0f } + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + bool topography_map; + xy_uint8_t grid_points; + #else // Bilinear + static constexpr xy_uint8_t grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y }; + #endif + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + float Z_offset; + bed_mesh_t z_values; + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; + float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations + eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points + mean; + #endif + #endif +}; + +#if ABL_USES_GRID && EITHER(AUTO_BED_LEVELING_3POINT, AUTO_BED_LEVELING_BILINEAR) + constexpr xy_uint8_t G29_State::grid_points; + constexpr int G29_State::abl_points; +#endif + +/** + * G29: Detailed Z probe, probes the bed at 3 or more points. + * Will fail if the printer has not been homed with G28. + * + * Enhanced G29 Auto Bed Leveling Probe Routine + * + * O Auto-level only if needed + * + * D Dry-Run mode. Just evaluate the bed Topology - Don't apply + * or alter the bed level data. Useful to check the topology + * after a first run of G29. + * + * J Jettison current bed leveling data + * + * V Set the verbose level (0-4). Example: "G29 V3" + * + * Parameters With LINEAR leveling only: + * + * P Set the size of the grid that will be probed (P x P points). + * Example: "G29 P4" + * + * X Set the X size of the grid that will be probed (X x Y points). + * Example: "G29 X7 Y5" + * + * Y Set the Y size of the grid that will be probed (X x Y points). + * + * T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report. + * This is useful for manual bed leveling and finding flaws in the bed (to + * assist with part placement). + * Not supported by non-linear delta printer bed leveling. + * + * Parameters With LINEAR and BILINEAR leveling only: + * + * S Set the XY travel speed between probe points (in units/min) + * + * H Set bounds to a centered square H x H units in size + * + * -or- + * + * F Set the Front limit of the probing grid + * B Set the Back limit of the probing grid + * L Set the Left limit of the probing grid + * R Set the Right limit of the probing grid + * + * Parameters with DEBUG_LEVELING_FEATURE only: + * + * C Make a totally fake grid with no actual probing. + * For use in testing when no probing is possible. + * + * Parameters with BILINEAR leveling only: + * + * Z Supply an additional Z probe offset + * + * Extra parameters with PROBE_MANUALLY: + * + * To do manual probing simply repeat G29 until the procedure is complete. + * The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort. + * + * Q Query leveling and G29 state + * + * A Abort current leveling procedure + * + * Extra parameters with BILINEAR only: + * + * W Write a mesh point. (If G29 is idle.) + * I X index for mesh point + * J Y index for mesh point + * X X for mesh point, overrides I + * Y Y for mesh point, overrides J + * Z Z for mesh point. Otherwise, raw current Z. + * + * Without PROBE_MANUALLY: + * + * E By default G29 will engage the Z probe, test the bed, then disengage. + * Include "E" to engage/disengage the Z probe for each sample. + * There's no extra effect if you have a fixed Z probe. + */ +G29_TYPE GcodeSuite::G29() { + DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING)); + + // Leveling state is persistent when done manually with multiple G29 commands + TERN_(PROBE_MANUALLY, static) G29_State abl; + + // Keep powered steppers from timing out + reset_stepper_timeout(); + + // Q = Query leveling and G29 state + const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen_test('Q'); + + // G29 Q is also available if debugging + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (seenQ || DEBUGGING(LEVELING)) log_machine_info(); + if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false, false); + #endif + + // A = Abort manual probing + // C = Generate fake probe points (DEBUG_LEVELING_FEATURE) + const bool seenA = TERN0(PROBE_MANUALLY, parser.seen_test('A')), + no_action = seenA || seenQ, + faux = ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY) ? parser.boolval('C') : no_action; + + // O = Don't level if leveling is already active + if (!no_action && planner.leveling_active && parser.boolval('O')) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Auto-level not needed, skip"); + G29_RETURN(false, false); + } + + // Send 'N' to force homing before G29 (internal only) + if (parser.seen_test('N')) + process_subcommands_now(TERN(CAN_SET_LEVELING_AFTER_G28, F("G28L0"), FPSTR(G28_STR))); + + // Don't allow auto-leveling without homing first + if (homing_needed_error()) G29_RETURN(false, false); + + // 3-point leveling gets points from the probe class + #if ENABLED(AUTO_BED_LEVELING_3POINT) + vector_3 points[3]; + probe.get_three_points(points); + #endif + + // Storage for ABL Linear results + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + struct linear_fit_data lsf_results; + #endif + + // Set and report "probing" state to host + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE, false)); + + /** + * On the initial G29 fetch command parameters. + */ + if (!g29_in_progress) { + + #if HAS_MULTI_HOTEND + abl.tool_index = active_extruder; + if (active_extruder != 0) tool_change(0, true); + #endif + + #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) + abl.abl_probe_index = -1; + #endif + + abl.reenable = planner.leveling_active; + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const bool seen_w = parser.seen_test('W'); + if (seen_w) { + if (!leveling_is_valid()) { + SERIAL_ERROR_MSG("No bilinear grid"); + G29_RETURN(false, false); + } + + const float rz = parser.seenval('Z') ? RAW_Z_POSITION(parser.value_linear_units()) : current_position.z; + if (!WITHIN(rz, -10, 10)) { + SERIAL_ERROR_MSG("Bad Z value"); + G29_RETURN(false, false); + } + + const float rx = RAW_X_POSITION(parser.linearval('X', NAN)), + ry = RAW_Y_POSITION(parser.linearval('Y', NAN)); + int8_t i = parser.byteval('I', -1), j = parser.byteval('J', -1); + + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" + + if (!isnan(rx) && !isnan(ry)) { + // Get nearest i / j from rx / ry + i = (rx - bedlevel.grid_start.x) / bedlevel.grid_spacing.x + 0.5f; + j = (ry - bedlevel.grid_start.y) / bedlevel.grid_spacing.y + 0.5f; + LIMIT(i, 0, (GRID_MAX_POINTS_X) - 1); + LIMIT(j, 0, (GRID_MAX_POINTS_Y) - 1); + } + + #pragma GCC diagnostic pop + + if (WITHIN(i, 0, (GRID_MAX_POINTS_X) - 1) && WITHIN(j, 0, (GRID_MAX_POINTS_Y) - 1)) { + set_bed_leveling_enabled(false); + bedlevel.z_values[i][j] = rz; + bedlevel.refresh_bed_level(); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz)); + if (abl.reenable) { + set_bed_leveling_enabled(true); + report_current_position(); + } + } + G29_RETURN(false, false); + } // parser.seen_test('W') + + #else + + constexpr bool seen_w = false; + + #endif + + // Jettison bed leveling data + if (!seen_w && parser.seen_test('J')) { + reset_bed_level(); + G29_RETURN(false, false); + } + + abl.verbose_level = parser.intval('V'); + if (!WITHIN(abl.verbose_level, 0, 4)) { + SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4)."); + G29_RETURN(false, false); + } + + abl.dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action); + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + incremental_LSF_reset(&lsf_results); + + abl.topography_map = abl.verbose_level > 2 || parser.boolval('T'); + + // X and Y specify points in each direction, overriding the default + // These values may be saved with the completed mesh + abl.grid_points.set( + parser.byteval('X', GRID_MAX_POINTS_X), + parser.byteval('Y', GRID_MAX_POINTS_Y) + ); + if (parser.seenval('P')) abl.grid_points.x = abl.grid_points.y = parser.value_int(); + + if (!WITHIN(abl.grid_points.x, 2, GRID_MAX_POINTS_X)) { + SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ")."); + G29_RETURN(false, false); + } + if (!WITHIN(abl.grid_points.y, 2, GRID_MAX_POINTS_Y)) { + SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ")."); + G29_RETURN(false, false); + } + + abl.abl_points = abl.grid_points.x * abl.grid_points.y; + abl.mean = 0; + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + abl.Z_offset = parser.linearval('Z'); + + #endif + + #if ABL_USES_GRID + + xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_FEEDRATE)); + + const float x_min = probe.min_x(), x_max = probe.max_x(), + y_min = probe.min_y(), y_max = probe.max_y(); + + if (parser.seen('H')) { + const int16_t size = (int16_t)parser.value_linear_units(); + abl.probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min)); + abl.probe_position_rb.set(_MIN(abl.probe_position_lf.x + size, x_max), _MIN(abl.probe_position_lf.y + size, y_max)); + } + else { + abl.probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min)); + abl.probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max)); + } + + if (!probe.good_bounds(abl.probe_position_lf, abl.probe_position_rb)) { + if (DEBUGGING(LEVELING)) { + DEBUG_ECHOLNPGM("G29 L", abl.probe_position_lf.x, " R", abl.probe_position_rb.x, + " F", abl.probe_position_lf.y, " B", abl.probe_position_rb.y); + } + SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds."); + G29_RETURN(false, false); + } + + // Probe at the points of a lattice grid + abl.gridSpacing.set((abl.probe_position_rb.x - abl.probe_position_lf.x) / (abl.grid_points.x - 1), + (abl.probe_position_rb.y - abl.probe_position_lf.y) / (abl.grid_points.y - 1)); + + #endif // ABL_USES_GRID + + if (abl.verbose_level > 0) { + SERIAL_ECHOPGM("G29 Auto Bed Leveling"); + if (abl.dryrun) SERIAL_ECHOPGM(" (DRYRUN)"); + SERIAL_EOL(); + } + + planner.synchronize(); + + #if ENABLED(AUTO_BED_LEVELING_3POINT) + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> 3-point Leveling"); + points[0].z = points[1].z = points[2].z = 0; // Probe at 3 arbitrary points + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + TERN_(DWIN_LCD_PROUI, DWIN_LevelingStart()); + #endif + + TERN_(EXTENSIBLE_UI, ExtUI::onLevelingStart()); + + if (!faux) { + remember_feedrate_scaling_off(); + + #if ENABLED(PREHEAT_BEFORE_LEVELING) + if (!abl.dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, + #if BOTH(DWIN_LCD_PROUI, HAS_HEATED_BED) + HMI_data.BedLevT + #else + LEVELING_BED_TEMP + #endif + ); + #endif + } + + // Disable auto bed leveling during G29. + // Be formal so G29 can be done successively without G28. + if (!no_action) set_bed_leveling_enabled(false); + + // Deploy certain probes before starting probing + #if ENABLED(BLTOUCH) + do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE); + #elif HAS_BED_PROBE + if (probe.deploy()) { // (returns true on deploy failure) + set_bed_leveling_enabled(abl.reenable); + G29_RETURN(false, true); + } + #endif + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + if (!abl.dryrun + && (abl.gridSpacing != bedlevel.grid_spacing || abl.probe_position_lf != bedlevel.grid_start) + ) { + // Reset grid to 0.0 or "not probed". (Also disables ABL) + reset_bed_level(); + + // Can't re-enable (on error) until the new grid is written + abl.reenable = false; + } + + // Pre-populate local Z values from the stored mesh + TERN_(IS_KINEMATIC, COPY(abl.z_values, bedlevel.z_values)); + + #endif // AUTO_BED_LEVELING_BILINEAR + + } // !g29_in_progress + + #if ENABLED(PROBE_MANUALLY) + + // For manual probing, get the next index to probe now. + // On the first probe this will be incremented to 0. + if (!no_action) { + ++abl.abl_probe_index; + g29_in_progress = true; + } + + // Abort current G29 procedure, go back to idle state + if (seenA && g29_in_progress) { + SERIAL_ECHOLNPGM("Manual G29 aborted"); + SET_SOFT_ENDSTOP_LOOSE(false); + set_bed_leveling_enabled(abl.reenable); + g29_in_progress = false; + TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); + } + + // Query G29 status + if (abl.verbose_level || seenQ) { + SERIAL_ECHOPGM("Manual G29 "); + if (g29_in_progress) + SERIAL_ECHOLNPGM("point ", _MIN(abl.abl_probe_index + 1, abl.abl_points), " of ", abl.abl_points); + else + SERIAL_ECHOLNPGM("idle"); + } + + // For 'A' or 'Q' exit with success state + if (no_action) G29_RETURN(false, true); + + if (abl.abl_probe_index == 0) { + // For the initial G29 S2 save software endstop state + SET_SOFT_ENDSTOP_LOOSE(true); + // Move close to the bed before the first point + do_blocking_move_to_z(0); + } + else { + + #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_3POINT) + const uint16_t index = abl.abl_probe_index - 1; + #endif + + // For G29 after adjusting Z. + // Save the previous Z before going to the next point + abl.measured_z = current_position.z; + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + abl.mean += abl.measured_z; + abl.eqnBVector[index] = abl.measured_z; + abl.eqnAMatrix[index + 0 * abl.abl_points] = abl.probePos.x; + abl.eqnAMatrix[index + 1 * abl.abl_points] = abl.probePos.y; + abl.eqnAMatrix[index + 2 * abl.abl_points] = 1; + + incremental_LSF(&lsf_results, abl.probePos, abl.measured_z); + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + points[index].z = abl.measured_z; + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const float newz = abl.measured_z + abl.Z_offset; + abl.z_values[abl.meshCount.x][abl.meshCount.y] = newz; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, newz)); + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM_P(PSTR("Save X"), abl.meshCount.x, SP_Y_STR, abl.meshCount.y, SP_Z_STR, abl.measured_z + abl.Z_offset); + + #endif + } + + // + // If there's another point to sample, move there with optional lift. + // + + #if ABL_USES_GRID + + // Skip any unreachable points + while (abl.abl_probe_index < abl.abl_points) { + + // Set abl.meshCount.x, abl.meshCount.y based on abl.abl_probe_index, with zig-zag + PR_OUTER_VAR = abl.abl_probe_index / PR_INNER_SIZE; + PR_INNER_VAR = abl.abl_probe_index - (PR_OUTER_VAR * PR_INNER_SIZE); + + // Probe in reverse order for every other row/column + const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_SIZE) & 1); + if (zig) PR_INNER_VAR = (PR_INNER_SIZE - 1) - PR_INNER_VAR; + + abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat(); + + TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = abl.abl_probe_index); + + // Keep looping till a reachable point is found + if (position_is_reachable(abl.probePos)) break; + ++abl.abl_probe_index; + } + + // Is there a next point to move to? + if (abl.abl_probe_index < abl.abl_points) { + _manual_goto_xy(abl.probePos); // Can be used here too! + // Disable software endstops to allow manual adjustment + // If G29 is not completed, they will not be re-enabled + SET_SOFT_ENDSTOP_LOOSE(true); + G29_RETURN(false, true); + } + else { + // Leveling done! Fall through to G29 finishing code below + SERIAL_ECHOLNPGM("Grid probing done."); + // Re-enable software endstops, if needed + SET_SOFT_ENDSTOP_LOOSE(false); + } + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + // Probe at 3 arbitrary points + if (abl.abl_probe_index < abl.abl_points) { + abl.probePos = xy_pos_t(points[abl.abl_probe_index]); + _manual_goto_xy(abl.probePos); + // Disable software endstops to allow manual adjustment + // If G29 is not completed, they will not be re-enabled + SET_SOFT_ENDSTOP_LOOSE(true); + G29_RETURN(false, true); + } + else { + + SERIAL_ECHOLNPGM("3-point probing done."); + + // Re-enable software endstops, if needed + SET_SOFT_ENDSTOP_LOOSE(false); + + if (!abl.dryrun) { + vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); + if (planeNormal.z < 0) planeNormal *= -1; + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + // Can't re-enable (on error) until the new grid is written + abl.reenable = false; + } + + } + + #endif // AUTO_BED_LEVELING_3POINT + + #else // !PROBE_MANUALLY + { + const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; + + abl.measured_z = 0; + + #if ABL_USES_GRID + + bool zig = PR_OUTER_SIZE & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION + + // Outer loop is X with PROBE_Y_FIRST enabled + // Outer loop is Y with PROBE_Y_FIRST disabled + for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_SIZE && !isnan(abl.measured_z); PR_OUTER_VAR++) { + + int8_t inStart, inStop, inInc; + + if (zig) { // Zig away from origin + inStart = 0; // Left or front + inStop = PR_INNER_SIZE; // Right or back + inInc = 1; // Zig right + } + else { // Zag towards origin + inStart = PR_INNER_SIZE - 1; // Right or back + inStop = -1; // Left or front + inInc = -1; // Zag left + } + + zig ^= true; // zag + + // An index to print current state + uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_SIZE) + 1; + + // Inner loop is Y with PROBE_Y_FIRST enabled + // Inner loop is X with PROBE_Y_FIRST disabled + for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) { + + abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat(); + + TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = ++abl.abl_probe_index); // 0... + + // Avoid probing outside the round or hexagonal area + if (TERN0(IS_KINEMATIC, !probe.can_reach(abl.probePos))) continue; + + if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing mesh point ", pt_index, "/", abl.abl_points, "."); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), int(pt_index), int(abl.abl_points))); + + abl.measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level); + + if (isnan(abl.measured_z)) { + set_bed_leveling_enabled(abl.reenable); + break; // Breaks out of both loops + } + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + + abl.mean += abl.measured_z; + abl.eqnBVector[abl.abl_probe_index] = abl.measured_z; + abl.eqnAMatrix[abl.abl_probe_index + 0 * abl.abl_points] = abl.probePos.x; + abl.eqnAMatrix[abl.abl_probe_index + 1 * abl.abl_points] = abl.probePos.y; + abl.eqnAMatrix[abl.abl_probe_index + 2 * abl.abl_points] = 1; + + incremental_LSF(&lsf_results, abl.probePos, abl.measured_z); + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + const float z = abl.measured_z + abl.Z_offset; + abl.z_values[abl.meshCount.x][abl.meshCount.y] = z; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, z)); + + #endif + + abl.reenable = false; // Don't re-enable after modifying the mesh + idle_no_sleep(); + + } // inner + } // outer + + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + + // Probe at 3 arbitrary points + + LOOP_L_N(i, 3) { + if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing point ", i + 1, "/3."); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_POINT), int(i + 1))); + + // Retain the last probe position + abl.probePos = xy_pos_t(points[i]); + abl.measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level); + if (isnan(abl.measured_z)) { + set_bed_leveling_enabled(abl.reenable); + break; + } + points[i].z = abl.measured_z; + } + + if (!abl.dryrun && !isnan(abl.measured_z)) { + vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); + if (planeNormal.z < 0) planeNormal *= -1; + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + // Can't re-enable (on error) until the new grid is written + abl.reenable = false; + } + + #endif // AUTO_BED_LEVELING_3POINT + + TERN_(HAS_STATUS_MESSAGE, ui.reset_status()); + + // Stow the probe. No raise for FIX_MOUNTED_PROBE. + if (probe.stow()) { + set_bed_leveling_enabled(abl.reenable); + abl.measured_z = NAN; + } + } + #endif // !PROBE_MANUALLY + + // + // G29 Finishing Code + // + // Unless this is a dry run, auto bed leveling will + // definitely be enabled after this point. + // + // If code above wants to continue leveling, it should + // return or loop before this point. + // + + if (DEBUGGING(LEVELING)) DEBUG_POS("> probing complete", current_position); + + #if ENABLED(PROBE_MANUALLY) + g29_in_progress = false; + TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); + #endif + + // Calculate leveling, print reports, correct the position + if (!isnan(abl.measured_z)) { + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + + if (abl.dryrun) + bedlevel.print_leveling_grid(&abl.z_values); + else { + bedlevel.set_grid(abl.gridSpacing, abl.probe_position_lf); + COPY(bedlevel.z_values, abl.z_values); + TERN_(IS_KINEMATIC, bedlevel.extrapolate_unprobed_bed_level()); + bedlevel.refresh_bed_level(); + + bedlevel.print_leveling_grid(); + } + + #elif ENABLED(AUTO_BED_LEVELING_LINEAR) + + // For LINEAR leveling calculate matrix, print reports, correct the position + + /** + * solve the plane equation ax + by + d = z + * A is the matrix with rows [x y 1] for all the probed points + * B is the vector of the Z positions + * the normal vector to the plane is formed by the coefficients of the + * plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 + * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z + */ + struct { float a, b, d; } plane_equation_coefficients; + + finish_incremental_LSF(&lsf_results); + plane_equation_coefficients.a = -lsf_results.A; // We should be able to eliminate the '-' on these three lines and down below + plane_equation_coefficients.b = -lsf_results.B; // but that is not yet tested. + plane_equation_coefficients.d = -lsf_results.D; + + abl.mean /= abl.abl_points; + + if (abl.verbose_level) { + SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8); + SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8); + SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8); + if (abl.verbose_level > 2) + SERIAL_ECHOPAIR_F("\nMean of sampled points: ", abl.mean, 8); + SERIAL_EOL(); + } + + // Create the matrix but don't correct the position yet + if (!abl.dryrun) + planner.bed_level_matrix = matrix_3x3::create_look_at( + vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1) // We can eliminate the '-' here and up above + ); + + // Show the Topography map if enabled + if (abl.topography_map) { + + float min_diff = 999; + + auto print_topo_map = [&](FSTR_P const title, const bool get_min) { + SERIAL_ECHOF(title); + for (int8_t yy = abl.grid_points.y - 1; yy >= 0; yy--) { + LOOP_L_N(xx, abl.grid_points.x) { + const int ind = abl.indexIntoAB[xx][yy]; + xyz_float_t tmp = { abl.eqnAMatrix[ind + 0 * abl.abl_points], + abl.eqnAMatrix[ind + 1 * abl.abl_points], 0 }; + planner.bed_level_matrix.apply_rotation_xyz(tmp.x, tmp.y, tmp.z); + if (get_min) NOMORE(min_diff, abl.eqnBVector[ind] - tmp.z); + const float subval = get_min ? abl.mean : tmp.z + min_diff, + diff = abl.eqnBVector[ind] - subval; + SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+'); // Include + for column alignment + SERIAL_ECHO_F(diff, 5); + } // xx + SERIAL_EOL(); + } // yy + SERIAL_EOL(); + }; + + print_topo_map(F("\nBed Height Topography:\n" + " +--- BACK --+\n" + " | |\n" + " L | (+) | R\n" + " E | | I\n" + " F | (-) N (+) | G\n" + " T | | H\n" + " | (-) | T\n" + " | |\n" + " O-- FRONT --+\n" + " (0,0)\n"), true); + if (abl.verbose_level > 3) + print_topo_map(F("\nCorrected Bed Height vs. Bed Topology:\n"), false); + + } // abl.topography_map + + #endif // AUTO_BED_LEVELING_LINEAR + + #if ABL_PLANAR + + // For LINEAR and 3POINT leveling correct the current position + + if (abl.verbose_level > 0) + planner.bed_level_matrix.debug(F("\n\nBed Level Correction Matrix:")); + + if (!abl.dryrun) { + // + // Correct the current XYZ position based on the tilted plane. + // + + if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position); + + xyze_pos_t converted = current_position; + planner.force_unapply_leveling(converted); // use conversion machinery + + // Use the last measured distance to the bed, if possible + if ( NEAR(current_position.x, abl.probePos.x - probe.offset_xy.x) + && NEAR(current_position.y, abl.probePos.y - probe.offset_xy.y) + ) { + const float simple_z = current_position.z - abl.measured_z; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z); + converted.z = simple_z; + } + + // The rotated XY and corrected Z are now current_position + current_position = converted; + + if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position); + + abl.reenable = true; + } + + // Auto Bed Leveling is complete! Enable if possible. + if (abl.reenable) { + planner.leveling_active = true; + sync_plan_position(); + } + + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + + // Auto Bed Leveling is complete! Enable if possible. + if (!abl.dryrun || abl.reenable) set_bed_leveling_enabled(true); + + #endif + + } // !isnan(abl.measured_z) + + // Restore state after probing + if (!faux) restore_feedrate_and_scaling(); + + TERN_(HAS_BED_PROBE, probe.move_z_after_probing()); + + #ifdef Z_PROBE_END_SCRIPT + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Probe End Script: ", Z_PROBE_END_SCRIPT); + planner.synchronize(); + process_subcommands_now(F(Z_PROBE_END_SCRIPT)); + #endif + + TERN_(HAS_MULTI_HOTEND, if (abl.tool_index != 0) tool_change(abl.tool_index)); + + report_current_position(); + + G29_RETURN(isnan(abl.measured_z), true); +} + +#endif // HAS_ABL_NOT_UBL diff --git a/src/gcode/bedlevel/abl/M421.cpp b/src/gcode/bedlevel/abl/M421.cpp new file mode 100644 index 0000000..3272ea1 --- /dev/null +++ b/src/gcode/bedlevel/abl/M421.cpp @@ -0,0 +1,74 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * M421.cpp - Auto Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(AUTO_BED_LEVELING_BILINEAR) + +#include "../../gcode.h" +#include "../../../feature/bedlevel/bedlevel.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#endif + +/** + * M421: Set one or more Mesh Bed Leveling Z coordinates + * + * Usage: + * M421 I J Z + * M421 I J Q + * + * - If I is omitted, set the entire row + * - If J is omitted, set the entire column + * - If both I and J are omitted, set all + */ +void GcodeSuite::M421() { + int8_t ix = parser.intval('I', -1), iy = parser.intval('J', -1); + const bool hasZ = parser.seenval('Z'), + hasQ = !hasZ && parser.seenval('Q'); + + if (hasZ || hasQ) { + if (WITHIN(ix, -1, GRID_MAX_POINTS_X - 1) && WITHIN(iy, -1, GRID_MAX_POINTS_Y - 1)) { + const float zval = parser.value_linear_units(); + uint8_t sx = ix >= 0 ? ix : 0, ex = ix >= 0 ? ix : GRID_MAX_POINTS_X - 1, + sy = iy >= 0 ? iy : 0, ey = iy >= 0 ? iy : GRID_MAX_POINTS_Y - 1; + LOOP_S_LE_N(x, sx, ex) { + LOOP_S_LE_N(y, sy, ey) { + bedlevel.z_values[x][y] = zval + (hasQ ? bedlevel.z_values[x][y] : 0); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, bedlevel.z_values[x][y])); + } + } + bedlevel.refresh_bed_level(); + } + else + SERIAL_ERROR_MSG(STR_ERR_MESH_XY); + } + else + SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS); +} + +#endif // AUTO_BED_LEVELING_BILINEAR diff --git a/src/gcode/bedlevel/mbl/G29.cpp b/src/gcode/bedlevel/mbl/G29.cpp new file mode 100644 index 0000000..227964e --- /dev/null +++ b/src/gcode/bedlevel/mbl/G29.cpp @@ -0,0 +1,229 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * G29.cpp - Mesh Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(MESH_BED_LEVELING) + +#include "../../../feature/bedlevel/bedlevel.h" + +#include "../../gcode.h" +#include "../../queue.h" + +#include "../../../libs/buzzer.h" +#include "../../../lcd/marlinui.h" +#include "../../../module/motion.h" +#include "../../../module/planner.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_LCD_PROUI) + #include "../../../lcd/e3v2/proui/dwin.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../../core/debug_out.h" + +// Save 130 bytes with non-duplication of PSTR +inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM(" not entered."); } + +/** + * G29: Mesh-based Z probe, probes a grid and produces a + * mesh to compensate for variable bed height + * + * Parameters With MESH_BED_LEVELING: + * + * S0 Report the current mesh values + * S1 Start probing mesh points + * S2 Probe the next mesh point + * S3 In Jn Zn.nn Manually modify a single point + * S4 Zn.nn Set z offset. Positive away from bed, negative closer to bed. + * S5 Reset and disable mesh + */ +void GcodeSuite::G29() { + DEBUG_SECTION(log_G29, "G29", true); + + // G29 Q is also available if debugging + #if ENABLED(DEBUG_LEVELING_FEATURE) + const bool seenQ = parser.seen_test('Q'); + if (seenQ || DEBUGGING(LEVELING)) { + log_machine_info(); + if (seenQ) return; + } + #endif + + static int mbl_probe_index = -1; + + MeshLevelingState state = (MeshLevelingState)parser.byteval('S', (int8_t)MeshReport); + if (!WITHIN(state, 0, 5)) { + SERIAL_ECHOLNPGM("S out of range (0-5)."); + return; + } + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); + + int8_t ix, iy; + ix = iy = 0; + + switch (state) { + case MeshReport: + SERIAL_ECHOPGM("Mesh Bed Leveling "); + if (leveling_is_valid()) { + serialprintln_onoff(planner.leveling_active); + bedlevel.report_mesh(); + } + else + SERIAL_ECHOLNPGM("has no data."); + break; + + case MeshStart: + bedlevel.reset(); + mbl_probe_index = 0; + if (!ui.wait_for_move) { + queue.inject(parser.seen_test('N') ? F("G28" TERN(CAN_SET_LEVELING_AFTER_G28, "L0", "") "\nG29S2") : F("G29S2")); + TERN_(EXTENSIBLE_UI, ExtUI::onLevelingStart()); + TERN_(DWIN_LCD_PROUI, DWIN_LevelingStart()); + return; + } + state = MeshNext; + + case MeshNext: + if (mbl_probe_index < 0) { + SERIAL_ECHOLNPGM("Start mesh probing with \"G29 S1\" first."); + return; + } + // For each G29 S2... + if (mbl_probe_index == 0) { + // Move close to the bed before the first point + do_blocking_move_to_z( + #ifdef MANUAL_PROBE_START_Z + MANUAL_PROBE_START_Z + #else + 0.4f + #endif + ); + } + else { + // Save Z for the previous mesh position + bedlevel.set_zigzag_z(mbl_probe_index - 1, current_position.z); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, current_position.z)); + TERN_(DWIN_LCD_PROUI, DWIN_MeshUpdate(_MIN(mbl_probe_index, GRID_MAX_POINTS), int(GRID_MAX_POINTS), current_position.z)); + SET_SOFT_ENDSTOP_LOOSE(false); + } + // If there's another point to sample, move there with optional lift. + if (mbl_probe_index < (GRID_MAX_POINTS)) { + // Disable software endstops to allow manual adjustment + // If G29 is left hanging without completion they won't be re-enabled! + SET_SOFT_ENDSTOP_LOOSE(true); + bedlevel.zigzag(mbl_probe_index++, ix, iy); + _manual_goto_xy({ bedlevel.index_to_xpos[ix], bedlevel.index_to_ypos[iy] }); + } + else { + // Move to the after probing position + current_position.z = ( + #ifdef Z_AFTER_PROBING + Z_AFTER_PROBING + #else + Z_CLEARANCE_BETWEEN_MANUAL_PROBES + #endif + ); + line_to_current_position(); + planner.synchronize(); + + // After recording the last point, activate home and activate + mbl_probe_index = -1; + SERIAL_ECHOLNPGM("Mesh probing done."); + TERN_(HAS_STATUS_MESSAGE, LCD_MESSAGE(MSG_MESH_DONE)); + OKAY_BUZZ(); + + home_all_axes(); + set_bed_leveling_enabled(true); + + #if ENABLED(MESH_G28_REST_ORIGIN) + current_position.z = 0; + line_to_current_position(homing_feedrate(Z_AXIS)); + planner.synchronize(); + #endif + + TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); + TERN_(EXTENSIBLE_UI, ExtUI::onLevelingDone()); + } + break; + + case MeshSet: + if (parser.seenval('I')) { + ix = parser.value_int(); + if (!WITHIN(ix, 0, (GRID_MAX_POINTS_X) - 1)) { + SERIAL_ECHOLNPGM("I out of range (0-", (GRID_MAX_POINTS_X) - 1, ")"); + return; + } + } + else + return echo_not_entered('J'); + + if (parser.seenval('J')) { + iy = parser.value_int(); + if (!WITHIN(iy, 0, (GRID_MAX_POINTS_Y) - 1)) { + SERIAL_ECHOLNPGM("J out of range (0-", (GRID_MAX_POINTS_Y) - 1, ")"); + return; + } + } + else + return echo_not_entered('J'); + + if (parser.seenval('Z')) { + bedlevel.z_values[ix][iy] = parser.value_linear_units(); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, bedlevel.z_values[ix][iy])); + TERN_(DWIN_LCD_PROUI, DWIN_MeshUpdate(ix, iy, bedlevel.z_values[ix][iy])); + } + else + return echo_not_entered('Z'); + break; + + case MeshSetZOffset: + if (parser.seenval('Z')) + bedlevel.z_offset = parser.value_linear_units(); + else + return echo_not_entered('Z'); + break; + + case MeshReset: + reset_bed_level(); + break; + + } // switch(state) + + if (state == MeshNext) { + SERIAL_ECHOLNPGM("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS), " of ", GRID_MAX_POINTS); + if (mbl_probe_index > 0) TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), _MIN(mbl_probe_index, GRID_MAX_POINTS), int(GRID_MAX_POINTS))); + } + + report_current_position(); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); +} + +#endif // MESH_BED_LEVELING diff --git a/src/gcode/bedlevel/mbl/M421.cpp b/src/gcode/bedlevel/mbl/M421.cpp new file mode 100644 index 0000000..e23683d --- /dev/null +++ b/src/gcode/bedlevel/mbl/M421.cpp @@ -0,0 +1,59 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * M421.cpp - Mesh Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(MESH_BED_LEVELING) + +#include "../../gcode.h" +#include "../../../module/motion.h" +#include "../../../feature/bedlevel/mbl/mesh_bed_leveling.h" + +/** + * M421: Set a single Mesh Bed Leveling Z coordinate + * + * Usage: + * M421 X Y Z + * M421 X Y Q + * M421 I J Z + * M421 I J Q + */ +void GcodeSuite::M421() { + const bool hasX = parser.seen('X'), hasI = parser.seen('I'); + const int8_t ix = hasI ? parser.value_int() : hasX ? bedlevel.probe_index_x(RAW_X_POSITION(parser.value_linear_units())) : -1; + const bool hasY = parser.seen('Y'), hasJ = parser.seen('J'); + const int8_t iy = hasJ ? parser.value_int() : hasY ? bedlevel.probe_index_y(RAW_Y_POSITION(parser.value_linear_units())) : -1; + const bool hasZ = parser.seen('Z'), hasQ = !hasZ && parser.seen('Q'); + + if (int(hasI && hasJ) + int(hasX && hasY) != 1 || !(hasZ || hasQ)) + SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS); + else if (ix < 0 || iy < 0) + SERIAL_ERROR_MSG(STR_ERR_MESH_XY); + else + bedlevel.set_z(ix, iy, parser.value_linear_units() + (hasQ ? bedlevel.z_values[ix][iy] : 0)); +} + +#endif // MESH_BED_LEVELING diff --git a/src/gcode/bedlevel/ubl/G29.cpp b/src/gcode/bedlevel/ubl/G29.cpp new file mode 100644 index 0000000..90deab3 --- /dev/null +++ b/src/gcode/bedlevel/ubl/G29.cpp @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * G29.cpp - Unified Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + +#include "../../gcode.h" +#include "../../../feature/bedlevel/bedlevel.h" + +#if ENABLED(FULL_REPORT_TO_HOST_FEATURE) + #include "../../../module/motion.h" +#endif + +void GcodeSuite::G29() { + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); + + bedlevel.G29(); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); +} + +#endif // AUTO_BED_LEVELING_UBL diff --git a/src/gcode/bedlevel/ubl/M421.cpp b/src/gcode/bedlevel/ubl/M421.cpp new file mode 100644 index 0000000..ff74f4c --- /dev/null +++ b/src/gcode/bedlevel/ubl/M421.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * M421.cpp - Unified Bed Leveling + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + +#include "../../gcode.h" +#include "../../../feature/bedlevel/bedlevel.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_LCD_PROUI) + #include "../../../lcd/e3v2/proui/dwin.h" +#endif + +/** + * M421: Set a single Mesh Bed Leveling Z coordinate + * + * Usage: + * M421 I J Z : Set the Mesh Point IJ to the Z value + * M421 I J Q : Add the Q value to the Mesh Point IJ + * M421 I J N : Set the Mesh Point IJ to NAN (not set) + * M421 C Z : Set the closest Mesh Point to the Z value + * M421 C Q : Add the Q value to the closest Mesh Point + */ +void GcodeSuite::M421() { + xy_int8_t ij = { int8_t(parser.intval('I', -1)), int8_t(parser.intval('J', -1)) }; + const bool hasI = ij.x >= 0, + hasJ = ij.y >= 0, + hasC = parser.seen_test('C'), + hasN = parser.seen_test('N'), + hasZ = parser.seen('Z'), + hasQ = !hasZ && parser.seen('Q'); + + if (hasC) ij = bedlevel.find_closest_mesh_point_of_type(CLOSEST, current_position); + + // Test for bad parameter combinations + if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN)) + SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS); + + // Test for I J out of range + else if (!WITHIN(ij.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(ij.y, 0, GRID_MAX_POINTS_Y - 1)) + SERIAL_ERROR_MSG(STR_ERR_MESH_XY); + else { + float &zval = bedlevel.z_values[ij.x][ij.y]; // Altering this Mesh Point + zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0); // N=NAN, Z=NEWVAL, or Q=ADDVAL + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval)); // Ping ExtUI in case it's showing the mesh + TERN_(DWIN_LCD_PROUI, DWIN_MeshUpdate(ij.x, ij.y, zval)); + } +} + +#endif // AUTO_BED_LEVELING_UBL diff --git a/src/gcode/calibrate/G28.cpp b/src/gcode/calibrate/G28.cpp new file mode 100644 index 0000000..f7b480a --- /dev/null +++ b/src/gcode/calibrate/G28.cpp @@ -0,0 +1,589 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#include "../gcode.h" + +#include "../../module/endstops.h" +#include "../../module/planner.h" +#include "../../module/stepper.h" // for various + +#if HAS_MULTI_HOTEND + #include "../../module/tool_change.h" +#endif + +#if HAS_LEVELING + #include "../../feature/bedlevel/bedlevel.h" +#endif + +#if ENABLED(SENSORLESS_HOMING) + #include "../../feature/tmc_util.h" +#endif + +#include "../../module/probe.h" + +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + +#include "../../lcd/marlinui.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD) + #include "../../lcd/e3v2/creality/dwin.h" +#elif ENABLED(DWIN_LCD_PROUI) + #include "../../lcd/e3v2/proui/dwin.h" +#endif + +#if HAS_L64XX // set L6470 absolute position registers to counts + #include "../../libs/L64XX/L64XX_Marlin.h" +#endif + +#if ENABLED(LASER_FEATURE) + #include "../../feature/spindle_laser.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +#if ENABLED(QUICK_HOME) + + static void quick_home_xy() { + + // Pretend the current position is 0,0 + current_position.set(0.0, 0.0); + sync_plan_position(); + + const int x_axis_home_dir = TOOL_X_HOME_DIR(active_extruder); + + // Use a higher diagonal feedrate so axes move at homing speed + const float minfr = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)), + fr_mm_s = HYPOT(minfr, minfr); + + #if ENABLED(SENSORLESS_HOMING) + sensorless_t stealth_states { + NUM_AXIS_LIST( + TERN0(X_SENSORLESS, tmc_enable_stallguard(stepperX)), + TERN0(Y_SENSORLESS, tmc_enable_stallguard(stepperY)), + false, false, false, false + ) + , TERN0(X2_SENSORLESS, tmc_enable_stallguard(stepperX2)) + , TERN0(Y2_SENSORLESS, tmc_enable_stallguard(stepperY2)) + }; + #endif + + do_blocking_move_to_xy(1.5 * max_length(X_AXIS) * x_axis_home_dir, 1.5 * max_length(Y_AXIS) * Y_HOME_DIR, fr_mm_s); + + endstops.validate_homing_move(); + + current_position.set(0.0, 0.0); + + #if ENABLED(SENSORLESS_HOMING) && DISABLED(ENDSTOPS_ALWAYS_ON_DEFAULT) + TERN_(X_SENSORLESS, tmc_disable_stallguard(stepperX, stealth_states.x)); + TERN_(X2_SENSORLESS, tmc_disable_stallguard(stepperX2, stealth_states.x2)); + TERN_(Y_SENSORLESS, tmc_disable_stallguard(stepperY, stealth_states.y)); + TERN_(Y2_SENSORLESS, tmc_disable_stallguard(stepperY2, stealth_states.y2)); + #endif + } + +#endif // QUICK_HOME + +#if ENABLED(Z_SAFE_HOMING) + + inline void home_z_safely() { + DEBUG_SECTION(log_G28, "home_z_safely", DEBUGGING(LEVELING)); + + // Disallow Z homing if X or Y homing is needed + if (homing_needed_error(_BV(X_AXIS) | _BV(Y_AXIS))) return; + + sync_plan_position(); + + /** + * Move the Z probe (or just the nozzle) to the safe homing point + * (Z is already at the right height) + */ + constexpr xy_float_t safe_homing_xy = { Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT }; + #if HAS_HOME_OFFSET + xy_float_t okay_homing_xy = safe_homing_xy; + okay_homing_xy -= home_offset; + #else + constexpr xy_float_t okay_homing_xy = safe_homing_xy; + #endif + + destination.set(okay_homing_xy, current_position.z); + + TERN_(HOMING_Z_WITH_PROBE, destination -= probe.offset_xy); + + if (position_is_reachable(destination)) { + + if (DEBUGGING(LEVELING)) DEBUG_POS("home_z_safely", destination); + + // Free the active extruder for movement + TERN_(DUAL_X_CARRIAGE, idex_set_parked(false)); + + TERN_(SENSORLESS_HOMING, safe_delay(500)); // Short delay needed to settle + + do_blocking_move_to_xy(destination); + homeaxis(Z_AXIS); + } + else { + LCD_MESSAGE(MSG_ZPROBE_OUT); + SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER); + } + } + +#endif // Z_SAFE_HOMING + +#if ENABLED(IMPROVE_HOMING_RELIABILITY) + + motion_state_t begin_slow_homing() { + motion_state_t motion_state{0}; + motion_state.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS], + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] + OPTARG(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS]) + ); + planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100; + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = 100); + #if HAS_CLASSIC_JERK + motion_state.jerk_state = planner.max_jerk; + planner.max_jerk.set(0, 0 OPTARG(DELTA, 0)); + #endif + planner.refresh_acceleration_rates(); + return motion_state; + } + + void end_slow_homing(const motion_state_t &motion_state) { + planner.settings.max_acceleration_mm_per_s2[X_AXIS] = motion_state.acceleration.x; + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = motion_state.acceleration.y; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = motion_state.acceleration.z); + TERN_(HAS_CLASSIC_JERK, planner.max_jerk = motion_state.jerk_state); + planner.refresh_acceleration_rates(); + } + +#endif // IMPROVE_HOMING_RELIABILITY + +/** + * G28: Home all axes according to settings + * + * Parameters + * + * None Home to all axes with no parameters. + * With QUICK_HOME enabled XY will home together, then Z. + * + * L Force leveling state ON (if possible) or OFF after homing (Requires RESTORE_LEVELING_AFTER_G28 or ENABLE_LEVELING_AFTER_G28) + * O Home only if the position is not known and trusted + * R Raise by n mm/inches before homing + * + * Cartesian/SCARA parameters + * + * X Home to the X endstop + * Y Home to the Y endstop + * Z Home to the Z endstop + */ +void GcodeSuite::G28() { + DEBUG_SECTION(log_G28, "G28", DEBUGGING(LEVELING)); + if (DEBUGGING(LEVELING)) log_machine_info(); + + /* + * Set the laser power to false to stop the planner from processing the current power setting. + */ + #if ENABLED(LASER_FEATURE) + planner.laser_inline.status.isPowered = false; + #endif + + #if ENABLED(DUAL_X_CARRIAGE) + bool IDEX_saved_duplication_state = extruder_duplication_enabled; + DualXMode IDEX_saved_mode = dual_x_carriage_mode; + #endif + + #if ENABLED(MARLIN_DEV_MODE) + if (parser.seen_test('S')) { + LOOP_NUM_AXES(a) set_axis_is_at_home((AxisEnum)a); + sync_plan_position(); + SERIAL_ECHOLNPGM("Simulated Homing"); + report_current_position(); + return; + } + #endif + + // Home (O)nly if position is unknown + if (!axes_should_home() && parser.seen_test('O')) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip"); + return; + } + + #if ENABLED(FULL_REPORT_TO_HOST_FEATURE) + const M_StateEnum old_grblstate = M_State_grbl; + set_and_report_grblstate(M_HOMING); + #endif + + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_HomingStart()); + TERN_(EXTENSIBLE_UI, ExtUI::onHomingStart()); + + planner.synchronize(); // Wait for planner moves to finish! + + SET_SOFT_ENDSTOP_LOOSE(false); // Reset a leftover 'loose' motion state + + // Disable the leveling matrix before homing + #if CAN_SET_LEVELING_AFTER_G28 + const bool leveling_restore_state = parser.boolval('L', TERN1(RESTORE_LEVELING_AFTER_G28, planner.leveling_active)); + #endif + + // Cancel any prior G29 session + TERN_(PROBE_MANUALLY, g29_in_progress = false); + + // Disable leveling before homing + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + + // Reset to the XY plane + TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY); + + // Count this command as movement / activity + reset_stepper_timeout(); + + #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT) + #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) || (ENABLED(DELTA) && HAS_CURRENT_HOME(Z)) || HAS_CURRENT_HOME(I) || HAS_CURRENT_HOME(J) || HAS_CURRENT_HOME(K) + #define HAS_HOMING_CURRENT 1 + #endif + + #if HAS_HOMING_CURRENT + auto debug_current = [](FSTR_P const s, const int16_t a, const int16_t b) { + DEBUG_ECHOF(s); DEBUG_ECHOLNPGM(" current: ", a, " -> ", b); + }; + #if HAS_CURRENT_HOME(X) + const int16_t tmc_save_current_X = stepperX.getMilliamps(); + stepperX.rms_current(X_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_X), tmc_save_current_X, X_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(X2) + const int16_t tmc_save_current_X2 = stepperX2.getMilliamps(); + stepperX2.rms_current(X2_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_X2), tmc_save_current_X2, X2_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(Y) + const int16_t tmc_save_current_Y = stepperY.getMilliamps(); + stepperY.rms_current(Y_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Y), tmc_save_current_Y, Y_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(Y2) + const int16_t tmc_save_current_Y2 = stepperY2.getMilliamps(); + stepperY2.rms_current(Y2_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Y2), tmc_save_current_Y2, Y2_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(I) + const int16_t tmc_save_current_I = stepperI.getMilliamps(); + stepperI.rms_current(I_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_I), tmc_save_current_I, I_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(J) + const int16_t tmc_save_current_J = stepperJ.getMilliamps(); + stepperJ.rms_current(J_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_J), tmc_save_current_J, J_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(K) + const int16_t tmc_save_current_K = stepperK.getMilliamps(); + stepperK.rms_current(K_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_K), tmc_save_current_K, K_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + const int16_t tmc_save_current_Z = stepperZ.getMilliamps(); + stepperZ.rms_current(Z_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Z), tmc_save_current_Z, Z_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(I) + const int16_t tmc_save_current_I = stepperI.getMilliamps(); + stepperI.rms_current(I_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_I), tmc_save_current_I, I_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(J) + const int16_t tmc_save_current_J = stepperJ.getMilliamps(); + stepperJ.rms_current(J_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_J), tmc_save_current_J, J_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(K) + const int16_t tmc_save_current_K = stepperK.getMilliamps(); + stepperK.rms_current(K_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_K), tmc_save_current_K, K_CURRENT_HOME); + #endif + #if SENSORLESS_STALLGUARD_DELAY + safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle + #endif + #endif + + #if ENABLED(IMPROVE_HOMING_RELIABILITY) + motion_state_t saved_motion_state = begin_slow_homing(); + #endif + + // Always home with tool 0 active + #if HAS_MULTI_HOTEND + #if DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE) + const uint8_t old_tool_index = active_extruder; + #endif + // PARKING_EXTRUDER homing requires different handling of movement / solenoid activation, depending on the side of homing + #if ENABLED(PARKING_EXTRUDER) + const bool pe_final_change_must_unpark = parking_extruder_unpark_after_homing(old_tool_index, X_HOME_DIR + 1 == old_tool_index * 2); + #endif + tool_change(0, true); + #endif + + TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false)); + + remember_feedrate_scaling_off(); + + endstops.enable(true); // Enable endstops for next homing move + + #if ENABLED(DELTA) + + constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a DELTA + + home_delta(); + + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); + + #elif ENABLED(AXEL_TPARA) + + constexpr bool doZ = true; // for NANODLP_Z_SYNC if your DLP is on a TPARA + + home_TPARA(); + + #else + + #define _UNSAFE(A) (homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(A##_AXIS)))) + + const bool homeZ = TERN0(HAS_Z_AXIS, parser.seen_test('Z')), + NUM_AXIS_LIST( // Other axes should be homed before Z safe-homing + needX = _UNSAFE(X), needY = _UNSAFE(Y), needZ = false, // UNUSED + needI = _UNSAFE(I), needJ = _UNSAFE(J), needK = _UNSAFE(K) + ), + NUM_AXIS_LIST( // Home each axis if needed or flagged + homeX = needX || parser.seen_test('X'), + homeY = needY || parser.seen_test('Y'), + homeZZ = homeZ, + homeI = needI || parser.seen_test(AXIS4_NAME), homeJ = needJ || parser.seen_test(AXIS5_NAME), homeK = needK || parser.seen_test(AXIS6_NAME) + ), + home_all = NUM_AXIS_GANG( // Home-all if all or none are flagged + homeX == homeX, && homeY == homeX, && homeZ == homeX, + && homeI == homeX, && homeJ == homeX, && homeK == homeX + ), + NUM_AXIS_LIST( + doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ, + doI = home_all || homeI, doJ = home_all || homeJ, doK = home_all || homeK + ); + + #if HAS_Z_AXIS + UNUSED(needZ); UNUSED(homeZZ); + #else + constexpr bool doZ = false; + #endif + + TERN_(HOME_Z_FIRST, if (doZ) homeaxis(Z_AXIS)); + + const bool seenR = parser.seenval('R'); + const float z_homing_height = seenR ? parser.value_linear_units() : Z_HOMING_HEIGHT; + + if (z_homing_height && (seenR || NUM_AXIS_GANG(doX, || doY, || TERN0(Z_SAFE_HOMING, doZ), || doI, || doJ, || doK))) { + // Raise Z before homing any other axes and z is not already high enough (never lower z) + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Raise Z (before homing) by ", z_homing_height); + do_z_clearance(z_homing_height); + TERN_(BLTOUCH, bltouch.init()); + } + + // Diagonal move first if both are homing + TERN_(QUICK_HOME, if (doX && doY) quick_home_xy()); + + // Home Y (before X) + if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX))) + homeaxis(Y_AXIS); + + // Home X + if (doX || (doY && ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X))) { + + #if ENABLED(DUAL_X_CARRIAGE) + + // Always home the 2nd (right) extruder first + active_extruder = 1; + homeaxis(X_AXIS); + + // Remember this extruder's position for later tool change + inactive_extruder_x = current_position.x; + + // Home the 1st (left) extruder + active_extruder = 0; + homeaxis(X_AXIS); + + // Consider the active extruder to be in its "parked" position + idex_set_parked(); + + #else + + homeaxis(X_AXIS); + + #endif + } + + // Home Y (after X) + if (DISABLED(HOME_Y_BEFORE_X) && doY) + homeaxis(Y_AXIS); + + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); + + // Home Z last if homing towards the bed + #if HAS_Z_AXIS && DISABLED(HOME_Z_FIRST) + if (doZ) { + #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN) + stepper.set_all_z_lock(false); + stepper.set_separate_multi_axis(false); + #endif + + #if ENABLED(Z_SAFE_HOMING) + if (TERN1(POWER_LOSS_RECOVERY, !parser.seen_test('H'))) home_z_safely(); else homeaxis(Z_AXIS); + #else + homeaxis(Z_AXIS); + #endif + probe.move_z_after_homing(); + } + #endif + + SECONDARY_AXIS_CODE( + if (doI) homeaxis(I_AXIS), + if (doJ) homeaxis(J_AXIS), + if (doK) homeaxis(K_AXIS) + ); + + sync_plan_position(); + + #endif + + /** + * Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE. + * This is important because it lets a user use the LCD Panel to set an IDEX Duplication mode, and + * then print a standard GCode file that contains a single print that does a G28 and has no other + * IDEX specific commands in it. + */ + #if ENABLED(DUAL_X_CARRIAGE) + + if (idex_is_duplicating()) { + + TERN_(IMPROVE_HOMING_RELIABILITY, saved_motion_state = begin_slow_homing()); + + // Always home the 2nd (right) extruder first + active_extruder = 1; + homeaxis(X_AXIS); + + // Remember this extruder's position for later tool change + inactive_extruder_x = current_position.x; + + // Home the 1st (left) extruder + active_extruder = 0; + homeaxis(X_AXIS); + + // Consider the active extruder to be parked + idex_set_parked(); + + dual_x_carriage_mode = IDEX_saved_mode; + set_duplication_enabled(IDEX_saved_duplication_state); + + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); + } + + #endif // DUAL_X_CARRIAGE + + endstops.not_homing(); + + // Clear endstop state for polled stallGuard endstops + TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state()); + + // Move to a height where we can use the full xy-area + TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height)); + + TERN_(CAN_SET_LEVELING_AFTER_G28, if (leveling_restore_state) set_bed_leveling_enabled()); + + restore_feedrate_and_scaling(); + + // Restore the active tool after homing + #if HAS_MULTI_HOTEND && (DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE)) + tool_change(old_tool_index, TERN(PARKING_EXTRUDER, !pe_final_change_must_unpark, DISABLED(DUAL_X_CARRIAGE))); // Do move if one of these + #endif + + #if HAS_HOMING_CURRENT + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore driver current..."); + #if HAS_CURRENT_HOME(X) + stepperX.rms_current(tmc_save_current_X); + #endif + #if HAS_CURRENT_HOME(X2) + stepperX2.rms_current(tmc_save_current_X2); + #endif + #if HAS_CURRENT_HOME(Y) + stepperY.rms_current(tmc_save_current_Y); + #endif + #if HAS_CURRENT_HOME(Y2) + stepperY2.rms_current(tmc_save_current_Y2); + #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + stepperZ.rms_current(tmc_save_current_Z); + #endif + #if HAS_CURRENT_HOME(I) + stepperI.rms_current(tmc_save_current_I); + #endif + #if HAS_CURRENT_HOME(J) + stepperJ.rms_current(tmc_save_current_J); + #endif + #if HAS_CURRENT_HOME(K) + stepperK.rms_current(tmc_save_current_K); + #endif + #if SENSORLESS_STALLGUARD_DELAY + safe_delay(SENSORLESS_STALLGUARD_DELAY); // Short delay needed to settle + #endif + #endif // HAS_HOMING_CURRENT + + ui.refresh(); + + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_HomingDone()); + TERN_(EXTENSIBLE_UI, ExtUI::onHomingDone()); + + report_current_position(); + + if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS))) + SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(old_grblstate)); + + #if HAS_L64XX + // Set L6470 absolute position registers to counts + // constexpr *might* move this to PROGMEM. + // If not, this will need a PROGMEM directive and an accessor. + #define _EN_ITEM(N) , E_AXIS + static constexpr AxisEnum L64XX_axis_xref[MAX_L64XX] = { + NUM_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS), + X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS, Z_AXIS + REPEAT(E_STEPPERS, _EN_ITEM) + }; + #undef _EN_ITEM + for (uint8_t j = 1; j <= L64XX::chain[0]; j++) { + const uint8_t cv = L64XX::chain[j]; + L64xxManager.set_param((L64XX_axis_t)cv, L6470_ABS_POS, stepper.position(L64XX_axis_xref[cv])); + } + #endif +} diff --git a/src/gcode/calibrate/G33.cpp b/src/gcode/calibrate/G33.cpp new file mode 100644 index 0000000..656c23c --- /dev/null +++ b/src/gcode/calibrate/G33.cpp @@ -0,0 +1,698 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(DELTA_AUTO_CALIBRATION) + +#include "../gcode.h" +#include "../../module/delta.h" +#include "../../module/motion.h" +#include "../../module/planner.h" +#include "../../module/endstops.h" +#include "../../lcd/marlinui.h" + +#if HAS_BED_PROBE + #include "../../module/probe.h" +#endif + +#if HAS_MULTI_HOTEND + #include "../../module/tool_change.h" +#endif + +#if HAS_LEVELING + #include "../../feature/bedlevel/bedlevel.h" +#endif + +constexpr uint8_t _7P_STEP = 1, // 7-point step - to change number of calibration points + _4P_STEP = _7P_STEP * 2, // 4-point step + NPP = _7P_STEP * 6; // number of calibration points on the radius +enum CalEnum : char { // the 7 main calibration points - add definitions if needed + CEN = 0, + __A = 1, + _AB = __A + _7P_STEP, + __B = _AB + _7P_STEP, + _BC = __B + _7P_STEP, + __C = _BC + _7P_STEP, + _CA = __C + _7P_STEP, +}; + +#define LOOP_CAL_PT(VAR, S, N) for (uint8_t VAR=S; VAR<=NPP; VAR+=N) +#define F_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VARCEN+0.9999; VAR-=N) +#define LOOP_CAL_ALL(VAR) LOOP_CAL_PT(VAR, CEN, 1) +#define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP) +#define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP) + +#if HAS_MULTI_HOTEND + const uint8_t old_tool_index = active_extruder; +#endif + +float lcd_probe_pt(const xy_pos_t &xy); + +void ac_home() { + endstops.enable(true); + TERN_(SENSORLESS_HOMING, endstops.set_homing_current(true)); + home_delta(); + TERN_(SENSORLESS_HOMING, endstops.set_homing_current(false)); + endstops.not_homing(); +} + +void ac_setup(const bool reset_bed) { + TERN_(HAS_MULTI_HOTEND, tool_change(0, true)); + + planner.synchronize(); + remember_feedrate_scaling_off(); + + #if HAS_LEVELING + if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid + #endif +} + +void ac_cleanup(TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index)) { + TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height)); + TERN_(HAS_BED_PROBE, probe.stow()); + restore_feedrate_and_scaling(); + TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, true)); +} + +void print_signed_float(FSTR_P const prefix, const_float_t f) { + SERIAL_ECHOPGM(" "); + SERIAL_ECHOF(prefix, AS_CHAR(':')); + serial_offset(f); +} + +/** + * - Print the delta settings + */ +static void print_calibration_settings(const bool end_stops, const bool tower_angles) { + SERIAL_ECHOPGM(".Height:", delta_height); + if (end_stops) { + print_signed_float(F("Ex"), delta_endstop_adj.a); + print_signed_float(F("Ey"), delta_endstop_adj.b); + print_signed_float(F("Ez"), delta_endstop_adj.c); + } + if (end_stops && tower_angles) { + SERIAL_ECHOLNPGM(" Radius:", delta_radius); + SERIAL_CHAR('.'); + SERIAL_ECHO_SP(13); + } + if (tower_angles) { + print_signed_float(F("Tx"), delta_tower_angle_trim.a); + print_signed_float(F("Ty"), delta_tower_angle_trim.b); + print_signed_float(F("Tz"), delta_tower_angle_trim.c); + } + if (end_stops != tower_angles) + SERIAL_ECHOPGM(" Radius:", delta_radius); + + SERIAL_EOL(); +} + +/** + * - Print the probe results + */ +static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) { + SERIAL_ECHOPGM(". "); + print_signed_float(F("c"), z_pt[CEN]); + if (tower_points) { + print_signed_float(F(" x"), z_pt[__A]); + print_signed_float(F(" y"), z_pt[__B]); + print_signed_float(F(" z"), z_pt[__C]); + } + if (tower_points && opposite_points) { + SERIAL_EOL(); + SERIAL_CHAR('.'); + SERIAL_ECHO_SP(13); + } + if (opposite_points) { + print_signed_float(F("yz"), z_pt[_BC]); + print_signed_float(F("zx"), z_pt[_CA]); + print_signed_float(F("xy"), z_pt[_AB]); + } + SERIAL_EOL(); +} + +/** + * - Calculate the standard deviation from the zero plane + */ +static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) { + if (!_0p_cal) { + float S2 = sq(z_pt[CEN]); + int16_t N = 1; + if (!_1p_cal) { // std dev from zero plane + LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) { + S2 += sq(z_pt[rad]); + N++; + } + return LROUND(SQRT(S2 / N) * 1000.0f) / 1000.0f + 0.00001f; + } + } + return 0.00001f; +} + +/** + * - Probe a point + */ +static float calibration_probe(const xy_pos_t &xy, const bool stow, const bool probe_at_offset) { + #if HAS_BED_PROBE + return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, probe_at_offset, false); + #else + UNUSED(stow); + return lcd_probe_pt(xy); + #endif +} + +/** + * - Probe a grid + */ +static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const float dcr, const bool towers_set, const bool stow_after_each, const bool probe_at_offset) { + const bool _0p_calibration = probe_points == 0, + _1p_calibration = probe_points == 1 || probe_points == -1, + _4p_calibration = probe_points == 2, + _4p_opposite_points = _4p_calibration && !towers_set, + _7p_calibration = probe_points >= 3, + _7p_no_intermediates = probe_points == 3, + _7p_1_intermediates = probe_points == 4, + _7p_2_intermediates = probe_points == 5, + _7p_4_intermediates = probe_points == 6, + _7p_6_intermediates = probe_points == 7, + _7p_8_intermediates = probe_points == 8, + _7p_11_intermediates = probe_points == 9, + _7p_14_intermediates = probe_points == 10, + _7p_intermed_points = probe_points >= 4, + _7p_6_center = probe_points >= 5 && probe_points <= 7, + _7p_9_center = probe_points >= 8; + + LOOP_CAL_ALL(rad) z_pt[rad] = 0.0f; + + if (!_0p_calibration) { + + if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center + const xy_pos_t center{0}; + z_pt[CEN] += calibration_probe(center, stow_after_each, probe_at_offset); + if (isnan(z_pt[CEN])) return false; + } + + if (_7p_calibration) { // probe extra center points + const float start = _7p_9_center ? float(_CA) + _7P_STEP / 3.0f : _7p_6_center ? float(_CA) : float(__C), + steps = _7p_9_center ? _4P_STEP / 3.0f : _7p_6_center ? _7P_STEP : _4P_STEP; + I_LOOP_CAL_PT(rad, start, steps) { + const float a = RADIANS(210 + (360 / NPP) * (rad - 1)), + r = dcr * 0.1; + const xy_pos_t vec = { cos(a), sin(a) }; + z_pt[CEN] += calibration_probe(vec * r, stow_after_each, probe_at_offset); + if (isnan(z_pt[CEN])) return false; + } + z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points); + } + + if (!_1p_calibration) { // probe the radius + const CalEnum start = _4p_opposite_points ? _AB : __A; + const float steps = _7p_14_intermediates ? _7P_STEP / 15.0f : // 15r * 6 + 10c = 100 + _7p_11_intermediates ? _7P_STEP / 12.0f : // 12r * 6 + 9c = 81 + _7p_8_intermediates ? _7P_STEP / 9.0f : // 9r * 6 + 10c = 64 + _7p_6_intermediates ? _7P_STEP / 7.0f : // 7r * 6 + 7c = 49 + _7p_4_intermediates ? _7P_STEP / 5.0f : // 5r * 6 + 6c = 36 + _7p_2_intermediates ? _7P_STEP / 3.0f : // 3r * 6 + 7c = 25 + _7p_1_intermediates ? _7P_STEP / 2.0f : // 2r * 6 + 4c = 16 + _7p_no_intermediates ? _7P_STEP : // 1r * 6 + 3c = 9 + _4P_STEP; // .5r * 6 + 1c = 4 + bool zig_zag = true; + F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) { + const int8_t offset = _7p_9_center ? 2 : 0; + for (int8_t circle = 0; circle <= offset; circle++) { + const float a = RADIANS(210 + (360 / NPP) * (rad - 1)), + r = dcr * (1 - 0.1 * (zig_zag ? offset - circle : circle)), + interpol = FMOD(rad, 1); + const xy_pos_t vec = { cos(a), sin(a) }; + const float z_temp = calibration_probe(vec * r, stow_after_each, probe_at_offset); + if (isnan(z_temp)) return false; + // split probe point to neighbouring calibration points + z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90))); + z_pt[uint8_t(LROUND(rad - interpol)) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90))); + } + zig_zag = !zig_zag; + } + if (_7p_intermed_points) + LOOP_CAL_RAD(rad) + z_pt[rad] /= _7P_STEP / steps; + + do_blocking_move_to_xy(0.0f, 0.0f); + } + } + return true; +} + +/** + * kinematics routines and auto tune matrix scaling parameters: + * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for + * - formulae for approximative forward kinematics in the end-stop displacement matrix + * - definition of the matrix scaling parameters + */ +static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1], const float dcr) { + xyz_pos_t pos{0}; + + LOOP_CAL_ALL(rad) { + const float a = RADIANS(210 + (360 / NPP) * (rad - 1)), + r = (rad == CEN ? 0.0f : dcr); + pos.set(cos(a) * r, sin(a) * r, z_pt[rad]); + inverse_kinematics(pos); + mm_at_pt_axis[rad] = delta; + } +} + +static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1], const float dcr) { + const float r_quot = dcr / delta_radius; + + #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A) + #define Z00(I, A) ZPP( 0, I, A) + #define Zp1(I, A) ZPP(+1, I, A) + #define Zm1(I, A) ZPP(-1, I, A) + #define Zp2(I, A) ZPP(+2, I, A) + #define Zm2(I, A) ZPP(-2, I, A) + + z_pt[CEN] = Z00(CEN, a) + Z00(CEN, b) + Z00(CEN, c); + z_pt[__A] = Zp2(__A, a) + Zm1(__A, b) + Zm1(__A, c); + z_pt[__B] = Zm1(__B, a) + Zp2(__B, b) + Zm1(__B, c); + z_pt[__C] = Zm1(__C, a) + Zm1(__C, b) + Zp2(__C, c); + z_pt[_BC] = Zm2(_BC, a) + Zp1(_BC, b) + Zp1(_BC, c); + z_pt[_CA] = Zp1(_CA, a) + Zm2(_CA, b) + Zp1(_CA, c); + z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c); +} + +static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], const float dcr, abc_float_t delta_e, const float delta_r, abc_float_t delta_t) { + const float z_center = z_pt[CEN]; + abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1]; + + reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis, dcr); + + delta_radius += delta_r; + delta_tower_angle_trim += delta_t; + recalc_delta_settings(); + reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis, dcr); + + LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e; + forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt, dcr); + + LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center; + z_pt[CEN] = z_center; + + delta_radius -= delta_r; + delta_tower_angle_trim -= delta_t; + recalc_delta_settings(); +} + +static float auto_tune_h(const float dcr) { + const float r_quot = dcr / delta_radius; + return RECIPROCAL(r_quot / (2.0f / 3.0f)); // (2/3)/CR +} + +static float auto_tune_r(const float dcr) { + constexpr float diff = 0.01f, delta_r = diff; + float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; + abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f }; + + calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t); + r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f; + r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z) + return r_fac; +} + +static float auto_tune_a(const float dcr) { + constexpr float diff = 0.01f, delta_r = 0.0f; + float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; + abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f }; + + delta_t.reset(); + LOOP_NUM_AXES(axis) { + delta_t[axis] = diff; + calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t); + delta_t[axis] = 0; + a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f; + a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f; + } + a_fac = diff / a_fac / 3.0f; // 1/(3*delta_Z) + return a_fac; +} + +/** + * G33 - Delta '1-4-7-point' Auto-Calibration + * Calibrate height, z_offset, endstops, delta radius, and tower angles. + * + * Parameters: + * + * Pn Number of probe points: + * P0 Normalizes calibration. + * P1 Calibrates height only with center probe. + * P2 Probe center and towers. Calibrate height, endstops and delta radius. + * P3 Probe all positions: center, towers and opposite towers. Calibrate all. + * P4-P10 Probe all positions at different intermediate locations and average them. + * + * Rn.nn Temporary reduce the probe grid by the specified amount (mm) + * + * T Don't calibrate tower angle corrections + * + * Cn.nn Calibration precision; when omitted calibrates to maximum precision + * + * Fn Force to run at least n iterations and take the best result + * + * Vn Verbose level: + * V0 Dry-run mode. Report settings and probe results. No calibration. + * V1 Report start and end settings only + * V2 Report settings at each iteration + * V3 Report settings and probe results + * + * E Engage the probe for each point + * + * O Probe at offsetted probe positions (this is wrong but it seems to work) + * + * With SENSORLESS_PROBING: + * Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.) + * X Don't activate stallguard on X. + * Y Don't activate stallguard on Y. + * Z Don't activate stallguard on Z. + * + * S Save offset_sensorless_adj + */ +void GcodeSuite::G33() { + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); + + const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS); + if (!WITHIN(probe_points, 0, 10)) { + SERIAL_ECHOLNPGM("?(P)oints implausible (0-10)."); + return; + } + + const bool probe_at_offset = TERN0(HAS_PROBE_XY_OFFSET, parser.seen_test('O')), + towers_set = !parser.seen_test('T'); + + // The calibration radius is set to a calculated value + float dcr = probe_at_offset ? DELTA_PRINTABLE_RADIUS : DELTA_PRINTABLE_RADIUS - PROBING_MARGIN; + #if HAS_PROBE_XY_OFFSET + const float total_offset = HYPOT(probe.offset_xy.x, probe.offset_xy.y); + dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset; + #endif + NOMORE(dcr, DELTA_PRINTABLE_RADIUS); + if (parser.seenval('R')) dcr -= _MAX(parser.value_float(), 0.0f); + TERN_(HAS_DELTA_SENSORLESS_PROBING, dcr *= sensorless_radius_factor); + + const float calibration_precision = parser.floatval('C', 0.0f); + if (calibration_precision < 0) { + SERIAL_ECHOLNPGM("?(C)alibration precision implausible (>=0)."); + return; + } + + const int8_t force_iterations = parser.intval('F', 0); + if (!WITHIN(force_iterations, 0, 30)) { + SERIAL_ECHOLNPGM("?(F)orce iteration implausible (0-30)."); + return; + } + + const int8_t verbose_level = parser.byteval('V', 1); + if (!WITHIN(verbose_level, 0, 3)) { + SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-3)."); + return; + } + + const bool stow_after_each = parser.seen_test('E'); + + #if HAS_DELTA_SENSORLESS_PROBING + probe.test_sensitivity = { !parser.seen_test('X'), !parser.seen_test('Y'), !parser.seen_test('Z') }; + const bool do_save_offset_adj = parser.seen_test('S'); + #endif + + const bool _0p_calibration = probe_points == 0, + _1p_calibration = probe_points == 1 || probe_points == -1, + _4p_calibration = probe_points == 2, + _4p_opposite_points = _4p_calibration && !towers_set, + _7p_9_center = probe_points >= 8, + _tower_results = (_4p_calibration && towers_set) || probe_points >= 3, + _opposite_results = (_4p_calibration && !towers_set) || probe_points >= 3, + _endstop_results = probe_points != 1 && probe_points != -1 && probe_points != 0, + _angle_results = probe_points >= 3 && towers_set; + int8_t iterations = 0; + float test_precision, + zero_std_dev = (verbose_level ? 999.0f : 0.0f), // 0.0 in dry-run mode : forced end + zero_std_dev_min = zero_std_dev, + zero_std_dev_old = zero_std_dev, + h_factor, r_factor, a_factor, + r_old = delta_radius, + h_old = delta_height; + + abc_pos_t e_old = delta_endstop_adj, a_old = delta_tower_angle_trim; + + SERIAL_ECHOLNPGM("G33 Auto Calibrate"); + + // Report settings + PGM_P const checkingac = PSTR("Checking... AC"); + SERIAL_ECHOPGM_P(checkingac); + SERIAL_ECHOPGM(" at radius:", dcr); + if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)"); + SERIAL_EOL(); + ui.set_status(checkingac); + + print_calibration_settings(_endstop_results, _angle_results); + + ac_setup(!_0p_calibration && !_1p_calibration); + + if (!_0p_calibration) ac_home(); + + #if HAS_DELTA_SENSORLESS_PROBING + if (verbose_level > 0 && do_save_offset_adj) { + offset_sensorless_adj.reset(); + + auto caltower = [&](Probe::sense_bool_t s){ + float z_at_pt[NPP + 1]; + LOOP_CAL_ALL(rad) z_at_pt[rad] = 0.0f; + probe.test_sensitivity = s; + if (probe_calibration_points(z_at_pt, 1, dcr, false, false, probe_at_offset)) + probe.set_offset_sensorless_adj(z_at_pt[CEN]); + }; + caltower({ true, false, false }); // A + caltower({ false, true, false }); // B + caltower({ false, false, true }); // C + + probe.test_sensitivity = { true, true, true }; // reset to all + } + #endif + + do { // start iterations + + float z_at_pt[NPP + 1] = { 0.0f }; + + test_precision = zero_std_dev_old != 999.0f ? (zero_std_dev + zero_std_dev_old) / 2.0f : zero_std_dev; + iterations++; + + // Probe the points + zero_std_dev_old = zero_std_dev; + if (!probe_calibration_points(z_at_pt, probe_points, dcr, towers_set, stow_after_each, probe_at_offset)) { + SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666"); + return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index)); + } + zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points); + + // Solve matrices + + if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) { + + #if !HAS_BED_PROBE + test_precision = 0.0f; // forced end + #endif + + if (zero_std_dev < zero_std_dev_min) { + // set roll-back point + e_old = delta_endstop_adj; + r_old = delta_radius; + h_old = delta_height; + a_old = delta_tower_angle_trim; + } + + abc_float_t e_delta = { 0.0f }, t_delta = { 0.0f }; + float r_delta = 0.0f; + + /** + * convergence matrices: + * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for + * - definition of the matrix scaling parameters + * - matrices for 4 and 7 point calibration + */ + #define ZP(N,I) ((N) * z_at_pt[I] / 4.0f) // 4.0 = divider to normalize to integers + #define Z12(I) ZP(12, I) + #define Z4(I) ZP(4, I) + #define Z2(I) ZP(2, I) + #define Z1(I) ZP(1, I) + #define Z0(I) ZP(0, I) + + // calculate factors + if (_7p_9_center) dcr *= 0.9f; + h_factor = auto_tune_h(dcr); + r_factor = auto_tune_r(dcr); + a_factor = auto_tune_a(dcr); + if (_7p_9_center) dcr /= 0.9f; + + switch (probe_points) { + case 0: + test_precision = 0.0f; // forced end + break; + + case 1: + test_precision = 0.0f; // forced end + LOOP_NUM_AXES(axis) e_delta[axis] = +Z4(CEN); + break; + + case 2: + if (towers_set) { // see 4 point calibration (towers) matrix + e_delta.set((+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN), + (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN), + (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN)); + r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor; + } + else { // see 4 point calibration (opposites) matrix + e_delta.set((-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN), + (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN), + (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN)); + r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor; + } + break; + + default: // see 7 point calibration (towers & opposites) matrix + e_delta.set((+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN), + (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN), + (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN)); + r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor; + + if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix + t_delta.set((+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor, + (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor, + (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor); + } + break; + } + delta_endstop_adj += e_delta; + delta_radius += r_delta; + delta_tower_angle_trim += t_delta; + } + else if (zero_std_dev >= test_precision) { + // roll back + delta_endstop_adj = e_old; + delta_radius = r_old; + delta_height = h_old; + delta_tower_angle_trim = a_old; + } + + if (verbose_level != 0) { // !dry run + + // Normalize angles to least-squares + if (_angle_results) { + float a_sum = 0.0f; + LOOP_NUM_AXES(axis) a_sum += delta_tower_angle_trim[axis]; + LOOP_NUM_AXES(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f; + } + + // adjust delta_height and endstops by the max amount + const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c); + delta_height -= z_temp; + LOOP_NUM_AXES(axis) delta_endstop_adj[axis] -= z_temp; + } + recalc_delta_settings(); + NOMORE(zero_std_dev_min, zero_std_dev); + + // print report + + if (verbose_level == 3 || verbose_level == 0) { + print_calibration_results(z_at_pt, _tower_results, _opposite_results); + #if HAS_DELTA_SENSORLESS_PROBING + if (verbose_level == 0 && probe_points == 1) { + if (do_save_offset_adj) + probe.set_offset_sensorless_adj(z_at_pt[CEN]); + else + probe.refresh_largest_sensorless_adj(); + } + #endif + } + + if (verbose_level != 0) { // !dry run + if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations + SERIAL_ECHOPGM("Calibration OK"); + SERIAL_ECHO_SP(32); + #if HAS_BED_PROBE + if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration) + SERIAL_ECHOPGM("rolling back."); + else + #endif + { + SERIAL_ECHOPAIR_F("std dev:", zero_std_dev_min, 3); + } + SERIAL_EOL(); + char mess[21]; + strcpy_P(mess, PSTR("Calibration sd:")); + if (zero_std_dev_min < 1) + sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev_min * 1000.0f)); + else + sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev_min)); + ui.set_status(mess); + print_calibration_settings(_endstop_results, _angle_results); + SERIAL_ECHOLNPGM("Save with M500 and/or copy to Configuration.h"); + } + else { // !end iterations + char mess[15]; + if (iterations < 31) + sprintf_P(mess, PSTR("Iteration : %02i"), (unsigned int)iterations); + else + strcpy_P(mess, PSTR("No convergence")); + SERIAL_ECHO(mess); + SERIAL_ECHO_SP(32); + SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3); + ui.set_status(mess); + if (verbose_level > 1) + print_calibration_settings(_endstop_results, _angle_results); + } + } + else { // dry run + FSTR_P const enddryrun = F("End DRY-RUN"); + SERIAL_ECHOF(enddryrun); + SERIAL_ECHO_SP(35); + SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3); + + char mess[21]; + strcpy_P(mess, FTOP(enddryrun)); + strcpy_P(&mess[11], PSTR(" sd:")); + if (zero_std_dev < 1) + sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0f)); + else + sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev)); + ui.set_status(mess); + } + ac_home(); + } + while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision); + + ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index)); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); + #if HAS_DELTA_SENSORLESS_PROBING + probe.test_sensitivity = { true, true, true }; + #endif +} + +#endif // DELTA_AUTO_CALIBRATION diff --git a/src/gcode/calibrate/G34.cpp b/src/gcode/calibrate/G34.cpp new file mode 100644 index 0000000..1be3952 --- /dev/null +++ b/src/gcode/calibrate/G34.cpp @@ -0,0 +1,160 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(MECHANICAL_GANTRY_CALIBRATION) + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/endstops.h" + +#if ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_TRINAMIC_CONFIG) + #include "../../module/stepper.h" +#endif + +#if HAS_LEVELING + #include "../../feature/bedlevel/bedlevel.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +void GcodeSuite::G34() { + + // Home before the alignment procedure + home_if_needed(); + + TERN_(HAS_LEVELING, TEMPORARY_BED_LEVELING_STATE(false)); + + SET_SOFT_ENDSTOP_LOOSE(true); + TemporaryGlobalEndstopsState unlock_z(false); + + #ifdef GANTRY_CALIBRATION_COMMANDS_PRE + process_subcommands_now(F(GANTRY_CALIBRATION_COMMANDS_PRE)); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed"); + #endif + + #ifdef GANTRY_CALIBRATION_SAFE_POSITION + // Move XY to safe position + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY"); + const xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION; + do_blocking_move_to(safe_pos, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE)); + #endif + + const float move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT), + zbase = ENABLED(GANTRY_CALIBRATION_TO_MIN) ? Z_MIN_POS : Z_MAX_POS, + zpounce = zbase - move_distance, zgrind = zbase + move_distance; + + // Move Z to pounce position + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce"); + do_blocking_move_to_z(zpounce, homing_feedrate(Z_AXIS)); + + // Store current motor settings, then apply reduced value + + #define _REDUCE_CURRENT ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_I2C, HAS_TRINAMIC_CONFIG) + #if _REDUCE_CURRENT + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current"); + #endif + + #if HAS_MOTOR_CURRENT_SPI + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS]; + stepper.set_digipot_current(Z_AXIS, target_current); + #elif HAS_MOTOR_CURRENT_PWM + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + const uint32_t previous_current = stepper.motor_current_setting[1]; // Z + stepper.set_digipot_current(1, target_current); + #elif HAS_MOTOR_CURRENT_DAC + const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT); + const float previous_current = dac_amps(Z_AXIS, target_current); + stepper_dac.set_current_value(Z_AXIS, target_current); + #elif HAS_MOTOR_CURRENT_I2C + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + previous_current = dac_amps(Z_AXIS); + digipot_i2c.set_current(Z_AXIS, target_current) + #elif HAS_TRINAMIC_CONFIG + const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); + static uint16_t previous_current_arr[NUM_Z_STEPPERS]; + #if AXIS_IS_TMC(Z) + previous_current_arr[0] = stepperZ.getMilliamps(); + stepperZ.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z2) + previous_current_arr[1] = stepperZ2.getMilliamps(); + stepperZ2.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z3) + previous_current_arr[2] = stepperZ3.getMilliamps(); + stepperZ3.rms_current(target_current); + #endif + #if AXIS_IS_TMC(Z4) + previous_current_arr[3] = stepperZ4.getMilliamps(); + stepperZ4.rms_current(target_current); + #endif + #endif + + // Do Final Z move to adjust + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move"); + do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); + + #if _REDUCE_CURRENT + // Reset current to original values + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current"); + #endif + + #if HAS_MOTOR_CURRENT_SPI + stepper.set_digipot_current(Z_AXIS, previous_current); + #elif HAS_MOTOR_CURRENT_PWM + stepper.set_digipot_current(1, previous_current); + #elif HAS_MOTOR_CURRENT_DAC + stepper_dac.set_current_value(Z_AXIS, previous_current); + #elif HAS_MOTOR_CURRENT_I2C + digipot_i2c.set_current(Z_AXIS, previous_current) + #elif HAS_TRINAMIC_CONFIG + #if AXIS_IS_TMC(Z) + stepperZ.rms_current(previous_current_arr[0]); + #endif + #if AXIS_IS_TMC(Z2) + stepperZ2.rms_current(previous_current_arr[1]); + #endif + #if AXIS_IS_TMC(Z3) + stepperZ3.rms_current(previous_current_arr[2]); + #endif + #if AXIS_IS_TMC(Z4) + stepperZ4.rms_current(previous_current_arr[3]); + #endif + #endif + + // Back off end plate, back to normal motion range + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff"); + do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); + + #ifdef GANTRY_CALIBRATION_COMMANDS_POST + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands"); + process_subcommands_now(F(GANTRY_CALIBRATION_COMMANDS_POST)); + #endif + + SET_SOFT_ENDSTOP_LOOSE(false); +} + +#endif // MECHANICAL_GANTRY_CALIBRATION diff --git a/src/gcode/calibrate/G34_M422.cpp b/src/gcode/calibrate/G34_M422.cpp new file mode 100644 index 0000000..8cf652c --- /dev/null +++ b/src/gcode/calibrate/G34_M422.cpp @@ -0,0 +1,569 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN) + +#include "../../feature/z_stepper_align.h" + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/stepper.h" +#include "../../module/planner.h" +#include "../../module/probe.h" +#include "../../lcd/marlinui.h" // for LCD_MESSAGE + +#if HAS_LEVELING + #include "../../feature/bedlevel/bedlevel.h" +#endif + +#if HAS_MULTI_HOTEND + #include "../../module/tool_change.h" +#endif + +#if HAS_Z_STEPPER_ALIGN_STEPPER_XY + #include "../../libs/least_squares_fit.h" +#endif + +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +#if NUM_Z_STEPPERS >= 3 + #define TRIPLE_Z 1 + #if NUM_Z_STEPPERS >= 4 + #define QUAD_Z 1 + #endif +#endif + +/** + * G34: Z-Stepper automatic alignment + * + * Manual stepper lock controls (reset by G28): + * L Unlock all steppers + * Z<1-4> Z stepper to lock / unlock + * S 0=UNLOCKED 1=LOCKED. If omitted, assume LOCKED. + * + * Examples: + * G34 Z1 ; Lock Z1 + * G34 L Z2 ; Unlock all, then lock Z2 + * G34 Z2 S0 ; Unlock Z2 + * + * With Z_STEPPER_AUTO_ALIGN: + * I Number of tests. If omitted, Z_STEPPER_ALIGN_ITERATIONS. + * T Target Accuracy factor. If omitted, Z_STEPPER_ALIGN_ACC. + * A Provide an Amplification value. If omitted, Z_STEPPER_ALIGN_AMP. + * R Flag to recalculate points based on current probe offsets + */ +void GcodeSuite::G34() { + DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING)); + if (DEBUGGING(LEVELING)) log_machine_info(); + + planner.synchronize(); // Prevent damage + + const bool seenL = parser.seen('L'); + if (seenL) stepper.set_all_z_lock(false); + + const bool seenZ = parser.seenval('Z'); + if (seenZ) { + const bool state = parser.boolval('S', true); + switch (parser.intval('Z')) { + case 1: stepper.set_z1_lock(state); break; + case 2: stepper.set_z2_lock(state); break; + #if TRIPLE_Z + case 3: stepper.set_z3_lock(state); break; + #if QUAD_Z + case 4: stepper.set_z4_lock(state); break; + #endif + #endif + } + } + + if (seenL || seenZ) { + stepper.set_separate_multi_axis(seenZ); + return; + } + + #if ENABLED(Z_STEPPER_AUTO_ALIGN) + do { // break out on error + + const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS); + if (!WITHIN(z_auto_align_iterations, 1, 30)) { + SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30)."); + break; + } + + const float z_auto_align_accuracy = parser.floatval('T', Z_STEPPER_ALIGN_ACC); + if (!WITHIN(z_auto_align_accuracy, 0.01f, 1.0f)) { + SERIAL_ECHOLNPGM("?(T)arget accuracy out of bounds (0.01-1.0)."); + break; + } + + const float z_auto_align_amplification = TERN(HAS_Z_STEPPER_ALIGN_STEPPER_XY, Z_STEPPER_ALIGN_AMP, parser.floatval('A', Z_STEPPER_ALIGN_AMP)); + if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) { + SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0)."); + break; + } + + if (parser.seen('R')) z_stepper_align.reset_to_default(); + + const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; + + // Disable the leveling matrix before auto-aligning + #if HAS_LEVELING + #if ENABLED(RESTORE_LEVELING_AFTER_G34) + const bool leveling_was_active = planner.leveling_active; + #endif + set_bed_leveling_enabled(false); + #endif + + TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY); + + // Always home with tool 0 active + #if HAS_MULTI_HOTEND + const uint8_t old_tool_index = active_extruder; + tool_change(0, true); + #endif + + TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false)); + + // In BLTOUCH HS mode, the probe travels in a deployed state. + // Users of G34 might have a badly misaligned bed, so raise Z by the + // length of the deployed pin (BLTOUCH stroke < 7mm) + #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + TERN0(BLTOUCH, bltouch.z_extra_clearance())) + + // Compute a worst-case clearance height to probe from. After the first + // iteration this will be re-calculated based on the actual bed position + auto magnitude2 = [&](const uint8_t i, const uint8_t j) { + const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j]; + return HYPOT2(diff.x, diff.y); + }; + float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(_MAX(0, magnitude2(0, 1) + #if TRIPLE_Z + , magnitude2(2, 1), magnitude2(2, 0) + #if QUAD_Z + , magnitude2(3, 2), magnitude2(3, 1), magnitude2(3, 0) + #endif + #endif + )); + + // Home before the alignment procedure + home_if_needed(); + + // Move the Z coordinate realm towards the positive - dirty trick + current_position.z += z_probe * 0.5f; + sync_plan_position(); + // Now, the Z origin lies below the build plate. That allows to probe deeper, before run_z_probe throws an error. + // This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration. + + #if !HAS_Z_STEPPER_ALIGN_STEPPER_XY + float last_z_align_move[NUM_Z_STEPPERS] = ARRAY_N_1(NUM_Z_STEPPERS, 10000.0f); + #else + float last_z_align_level_indicator = 10000.0f; + #endif + float z_measured[NUM_Z_STEPPERS] = { 0 }, + z_maxdiff = 0.0f, + amplification = z_auto_align_amplification; + + #if !HAS_Z_STEPPER_ALIGN_STEPPER_XY + bool adjustment_reverse = false; + #endif + + #if HAS_STATUS_MESSAGE + PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION); + const uint8_t iter_str_len = strlen_P(msg_iteration); + #endif + + // Final z and iteration values will be used after breaking the loop + float z_measured_min; + uint8_t iteration = 0; + bool err_break = false; // To break out of nested loops + while (iteration < z_auto_align_iterations) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions."); + + const int iter = iteration + 1; + SERIAL_ECHOLNPGM("\nG34 Iteration: ", iter); + #if HAS_STATUS_MESSAGE + char str[iter_str_len + 2 + 1]; + sprintf_P(str, msg_iteration, iter); + ui.set_status(str); + #endif + + // Initialize minimum value + z_measured_min = 100000.0f; + float z_measured_max = -100000.0f; + + // Probe all positions (one per Z-Stepper) + LOOP_L_N(i, NUM_Z_STEPPERS) { + // iteration odd/even --> downward / upward stepper sequence + const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPERS - 1 - i : i; + + // Safe clearance even on an incline + if ((iteration == 0 || i > 0) && z_probe > current_position.z) do_blocking_move_to_z(z_probe); + + xy_pos_t &ppos = z_stepper_align.xy[iprobe]; + + if (DEBUGGING(LEVELING)) + DEBUG_ECHOLNPGM_P(PSTR("Probing X"), ppos.x, SP_Y_STR, ppos.y); + + // Probe a Z height for each stepper. + // Probing sanity check is disabled, as it would trigger even in normal cases because + // current_position.z has been manually altered in the "dirty trick" above. + const float z_probed_height = probe.probe_at_point(DIFF_TERN(HAS_HOME_OFFSET, ppos, xy_pos_t(home_offset)), raise_after, 0, true, false); + if (isnan(z_probed_height)) { + SERIAL_ECHOLNPGM("Probing failed"); + LCD_MESSAGE(MSG_LCD_PROBING_FAILED); + err_break = true; + break; + } + + // Add height to each value, to provide a more useful target height for + // the next iteration of probing. This allows adjustments to be made away from the bed. + z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES; + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", iprobe + 1, " measured position is ", z_measured[iprobe]); + + // Remember the minimum measurement to calculate the correction later on + z_measured_min = _MIN(z_measured_min, z_measured[iprobe]); + z_measured_max = _MAX(z_measured_max, z_measured[iprobe]); + } // for (i) + + if (err_break) break; + + // Adapt the next probe clearance height based on the new measurements. + // Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment. + z_maxdiff = z_measured_max - z_measured_min; + z_probe = Z_BASIC_CLEARANCE + z_measured_max + z_maxdiff; + + #if HAS_Z_STEPPER_ALIGN_STEPPER_XY + // Replace the initial values in z_measured with calculated heights at + // each stepper position. This allows the adjustment algorithm to be + // shared between both possible probing mechanisms. + + // This must be done after the next z_probe height is calculated, so that + // the height is calculated from actual print area positions, and not + // extrapolated motor movements. + + // Compute the least-squares fit for all probed points. + // Calculate the Z position of each stepper and store it in z_measured. + // This allows the actual adjustment logic to be shared by both algorithms. + linear_fit_data lfd; + incremental_LSF_reset(&lfd); + LOOP_L_N(i, NUM_Z_STEPPERS) { + SERIAL_ECHOLNPGM("PROBEPT_", i, ": ", z_measured[i]); + incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]); + } + finish_incremental_LSF(&lfd); + + z_measured_min = 100000.0f; + LOOP_L_N(i, NUM_Z_STEPPERS) { + z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y + lfd.D); + z_measured_min = _MIN(z_measured_min, z_measured[i]); + } + + SERIAL_ECHOLNPGM( + LIST_N(DOUBLE(NUM_Z_STEPPERS), + "Calculated Z1=", z_measured[0], + " Z2=", z_measured[1], + " Z3=", z_measured[2], + " Z4=", z_measured[3] + ) + ); + #endif + + SERIAL_ECHOLNPGM("\n" + "Z2-Z1=", ABS(z_measured[1] - z_measured[0]) + #if TRIPLE_Z + , " Z3-Z2=", ABS(z_measured[2] - z_measured[1]) + , " Z3-Z1=", ABS(z_measured[2] - z_measured[0]) + #if QUAD_Z + , " Z4-Z3=", ABS(z_measured[3] - z_measured[2]) + , " Z4-Z2=", ABS(z_measured[3] - z_measured[1]) + , " Z4-Z1=", ABS(z_measured[3] - z_measured[0]) + #endif + #endif + ); + + #if HAS_STATUS_MESSAGE + char fstr1[10]; + char msg[6 + (6 + 5) * NUM_Z_STEPPERS + 1] + #if TRIPLE_Z + , fstr2[10], fstr3[10] + #if QUAD_Z + , fstr4[10], fstr5[10], fstr6[10] + #endif + #endif + ; + sprintf_P(msg, + PSTR("1:2=%s" TERN_(TRIPLE_Z, " 3-2=%s 3-1=%s") TERN_(QUAD_Z, " 4-3=%s 4-2=%s 4-1=%s")), + dtostrf(ABS(z_measured[1] - z_measured[0]), 1, 3, fstr1) + OPTARG(TRIPLE_Z, + dtostrf(ABS(z_measured[2] - z_measured[1]), 1, 3, fstr2), + dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)) + OPTARG(QUAD_Z, + dtostrf(ABS(z_measured[3] - z_measured[2]), 1, 3, fstr4), + dtostrf(ABS(z_measured[3] - z_measured[1]), 1, 3, fstr5), + dtostrf(ABS(z_measured[3] - z_measured[0]), 1, 3, fstr6)) + ); + ui.set_status(msg); + #endif + + auto decreasing_accuracy = [](const_float_t v1, const_float_t v2) { + if (v1 < v2 * 0.7f) { + SERIAL_ECHOLNPGM("Decreasing Accuracy Detected."); + LCD_MESSAGE(MSG_DECREASING_ACCURACY); + return true; + } + return false; + }; + + #if HAS_Z_STEPPER_ALIGN_STEPPER_XY + // Check if the applied corrections go in the correct direction. + // Calculate the sum of the absolute deviations from the mean of the probe measurements. + // Compare to the last iteration to ensure it's getting better. + + // Calculate mean value as a reference + float z_measured_mean = 0.0f; + LOOP_L_N(zstepper, NUM_Z_STEPPERS) z_measured_mean += z_measured[zstepper]; + z_measured_mean /= NUM_Z_STEPPERS; + + // Calculate the sum of the absolute deviations from the mean value + float z_align_level_indicator = 0.0f; + LOOP_L_N(zstepper, NUM_Z_STEPPERS) + z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean); + + // If it's getting worse, stop and throw an error + err_break = decreasing_accuracy(last_z_align_level_indicator, z_align_level_indicator); + if (err_break) break; + + last_z_align_level_indicator = z_align_level_indicator; + #endif + + // The following correction actions are to be enabled for select Z-steppers only + stepper.set_separate_multi_axis(true); + + bool success_break = true; + // Correct the individual stepper offsets + LOOP_L_N(zstepper, NUM_Z_STEPPERS) { + // Calculate current stepper move + float z_align_move = z_measured[zstepper] - z_measured_min; + const float z_align_abs = ABS(z_align_move); + + #if !HAS_Z_STEPPER_ALIGN_STEPPER_XY + // Optimize one iteration's correction based on the first measurements + if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification; + + // Check for less accuracy compared to last move + if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " last_z_align_move = ", last_z_align_move[zstepper]); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " z_align_abs = ", z_align_abs); + adjustment_reverse = !adjustment_reverse; + } + + // Remember the alignment for the next iteration, but only if steppers move, + // otherwise it would be just zero (in case this stepper was at z_measured_min already) + if (z_align_abs > 0) last_z_align_move[zstepper] = z_align_abs; + #endif + + // Stop early if all measured points achieve accuracy target + if (z_align_abs > z_auto_align_accuracy) success_break = false; + + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " corrected by ", z_align_move); + + // Lock all steppers except one + stepper.set_all_z_lock(true, zstepper); + + #if !HAS_Z_STEPPER_ALIGN_STEPPER_XY + // Decreasing accuracy was detected so move was inverted. + // Will match reversed Z steppers on dual steppers. Triple will need more work to map. + if (adjustment_reverse) { + z_align_move = -z_align_move; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " correction reversed to ", z_align_move); + } + #endif + + // Do a move to correct part of the misalignment for the current stepper + do_blocking_move_to_z(amplification * z_align_move + current_position.z); + } // for (zstepper) + + // Back to normal stepper operations + stepper.set_all_z_lock(false); + stepper.set_separate_multi_axis(false); + + if (err_break) break; + + if (success_break) { + SERIAL_ECHOLNPGM("Target accuracy achieved."); + LCD_MESSAGE(MSG_ACCURACY_ACHIEVED); + break; + } + + iteration++; + } // while (iteration < z_auto_align_iterations) + + if (err_break) + SERIAL_ECHOLNPGM("G34 aborted."); + else { + SERIAL_ECHOLNPGM("Did ", iteration + (iteration != z_auto_align_iterations), " of ", z_auto_align_iterations); + SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); + } + + // Stow the probe because the last call to probe.probe_at_point(...) + // leaves the probe deployed when it's successful. + IF_DISABLED(TOUCH_MI_PROBE, probe.stow()); + + #if ENABLED(HOME_AFTER_G34) + // After this operation the z position needs correction + set_axis_never_homed(Z_AXIS); + // Home Z after the alignment procedure + process_subcommands_now(F("G28Z")); + #else + // Use the probed height from the last iteration to determine the Z height. + // z_measured_min is used, because all steppers are aligned to z_measured_min. + // Ideally, this would be equal to the 'z_probe * 0.5f' which was added earlier. + current_position.z -= z_measured_min - (float)Z_CLEARANCE_BETWEEN_PROBES; + sync_plan_position(); + #endif + + // Restore the active tool after homing + TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER))); // Fetch previous tool for parking extruder + + #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G34) + set_bed_leveling_enabled(leveling_was_active); + #endif + + }while(0); + #endif // Z_STEPPER_AUTO_ALIGN +} + +#endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN + +#if ENABLED(Z_STEPPER_AUTO_ALIGN) + +/** + * M422: Set a Z-Stepper automatic alignment XY point. + * Use repeatedly to set multiple points. + * + * S : Index of the probe point to set + * + * With Z_STEPPER_ALIGN_STEPPER_XY: + * W : Index of the Z stepper position to set + * The W and S parameters may not be combined. + * + * S and W require an X and/or Y parameter + * X : X position to set (Unchanged if omitted) + * Y : Y position to set (Unchanged if omitted) + * + * R : Recalculate points based on current probe offsets + */ +void GcodeSuite::M422() { + + if (!parser.seen_any()) return M422_report(); + + if (parser.seen('R')) { + z_stepper_align.reset_to_default(); + return; + } + + const bool is_probe_point = parser.seen_test('S'); + + if (TERN0(HAS_Z_STEPPER_ALIGN_STEPPER_XY, is_probe_point && parser.seen_test('W'))) { + SERIAL_ECHOLNPGM("?(S) and (W) may not be combined."); + return; + } + + xy_pos_t * const pos_dest = ( + TERN_(HAS_Z_STEPPER_ALIGN_STEPPER_XY, !is_probe_point ? z_stepper_align.stepper_xy :) + z_stepper_align.xy + ); + + if (!is_probe_point && TERN1(HAS_Z_STEPPER_ALIGN_STEPPER_XY, !parser.seen_test('W'))) { + SERIAL_ECHOLNPGM("?(S)" TERN_(HAS_Z_STEPPER_ALIGN_STEPPER_XY, " or (W)") " is required."); + return; + } + + // Get the Probe Position Index or Z Stepper Index + int8_t position_index = 1; + FSTR_P err_string = F("?(S) Probe-position"); + if (is_probe_point) + position_index = parser.intval('S'); + else { + #if HAS_Z_STEPPER_ALIGN_STEPPER_XY + err_string = F("?(W) Z-stepper"); + position_index = parser.intval('W'); + #endif + } + + if (!WITHIN(position_index, 1, NUM_Z_STEPPERS)) { + SERIAL_ECHOF(err_string); + SERIAL_ECHOLNPGM(" index invalid (1.." STRINGIFY(NUM_Z_STEPPERS) ")."); + return; + } + + --position_index; + + const xy_pos_t pos = { + parser.floatval('X', pos_dest[position_index].x), + parser.floatval('Y', pos_dest[position_index].y) + }; + + if (is_probe_point) { + if (!probe.can_reach(pos.x, Y_CENTER)) { + SERIAL_ECHOLNPGM("?(X) out of bounds."); + return; + } + if (!probe.can_reach(pos)) { + SERIAL_ECHOLNPGM("?(Y) out of bounds."); + return; + } + } + + pos_dest[position_index] = pos; +} + +void GcodeSuite::M422_report(const bool forReplay/*=true*/) { + report_heading(forReplay, F(STR_Z_AUTO_ALIGN)); + LOOP_L_N(i, NUM_Z_STEPPERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M422 S"), i + 1, + SP_X_STR, z_stepper_align.xy[i].x, + SP_Y_STR, z_stepper_align.xy[i].y + ); + } + #if HAS_Z_STEPPER_ALIGN_STEPPER_XY + LOOP_L_N(i, NUM_Z_STEPPERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M422 W"), i + 1, + SP_X_STR, z_stepper_align.stepper_xy[i].x, + SP_Y_STR, z_stepper_align.stepper_xy[i].y + ); + } + #endif +} + +#endif // Z_STEPPER_AUTO_ALIGN diff --git a/src/gcode/calibrate/G425.cpp b/src/gcode/calibrate/G425.cpp new file mode 100644 index 0000000..f4d05ae --- /dev/null +++ b/src/gcode/calibrate/G425.cpp @@ -0,0 +1,764 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../MarlinCore.h" + +#if ENABLED(CALIBRATION_GCODE) + +#include "../gcode.h" + +#if ENABLED(BACKLASH_GCODE) + #include "../../feature/backlash.h" +#endif + +#include "../../lcd/marlinui.h" +#include "../../module/motion.h" +#include "../../module/planner.h" +#include "../../module/tool_change.h" +#include "../../module/endstops.h" +#include "../../feature/bedlevel/bedlevel.h" + +#if !AXIS_CAN_CALIBRATE(X) + #undef CALIBRATION_MEASURE_LEFT + #undef CALIBRATION_MEASURE_RIGHT +#endif + +#if !AXIS_CAN_CALIBRATE(Y) + #undef CALIBRATION_MEASURE_FRONT + #undef CALIBRATION_MEASURE_BACK +#endif + +#if !AXIS_CAN_CALIBRATE(Z) + #undef CALIBRATION_MEASURE_AT_TOP_EDGES +#endif + +/** + * G425 backs away from the calibration object by various distances + * depending on the confidence level: + * + * UNKNOWN - No real notion on where the calibration object is on the bed + * UNCERTAIN - Measurement may be uncertain due to backlash + * CERTAIN - Measurement obtained with backlash compensation + */ + +#ifndef CALIBRATION_MEASUREMENT_UNKNOWN + #define CALIBRATION_MEASUREMENT_UNKNOWN 5.0 // mm +#endif +#ifndef CALIBRATION_MEASUREMENT_UNCERTAIN + #define CALIBRATION_MEASUREMENT_UNCERTAIN 1.0 // mm +#endif +#ifndef CALIBRATION_MEASUREMENT_CERTAIN + #define CALIBRATION_MEASUREMENT_CERTAIN 0.5 // mm +#endif + +#if BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT) + #define HAS_X_CENTER 1 +#endif +#if ALL(HAS_Y_AXIS, CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK) + #define HAS_Y_CENTER 1 +#endif +#if ALL(HAS_I_AXIS, CALIBRATION_MEASURE_IMIN, CALIBRATION_MEASURE_IMAX) + #define HAS_I_CENTER 1 +#endif +#if ALL(HAS_J_AXIS, CALIBRATION_MEASURE_JMIN, CALIBRATION_MEASURE_JMAX) + #define HAS_J_CENTER 1 +#endif +#if ALL(HAS_K_AXIS, CALIBRATION_MEASURE_KMIN, CALIBRATION_MEASURE_KMAX) + #define HAS_K_CENTER 1 +#endif + +enum side_t : uint8_t { + TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES, + LIST_N(DOUBLE(SECONDARY_AXES), IMINIMUM, IMAXIMUM, JMINIMUM, JMAXIMUM, KMINIMUM, KMAXIMUM) +}; + +static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER; +static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS; +static constexpr xy_float_t nod = { CALIBRATION_NOZZLE_OUTER_DIAMETER, CALIBRATION_NOZZLE_OUTER_DIAMETER }; + +struct measurements_t { + xyz_pos_t obj_center = true_center; // Non-static must be assigned from xyz_pos_t + + float obj_side[NUM_SIDES], backlash[NUM_SIDES]; + xyz_float_t pos_error; + + xy_float_t nozzle_outer_dimension = nod; +}; + +#if ENABLED(BACKLASH_GCODE) + class restorer_correction { + const uint8_t val_; + public: + restorer_correction(const uint8_t temp_val) : val_(backlash.get_correction_uint8()) { backlash.set_correction_uint8(temp_val); } + ~restorer_correction() { backlash.set_correction_uint8(val_); } + }; + + #define TEMPORARY_BACKLASH_CORRECTION(value) restorer_correction restorer_tbst(value) +#else + #define TEMPORARY_BACKLASH_CORRECTION(value) +#endif + +#if ENABLED(BACKLASH_GCODE) && defined(BACKLASH_SMOOTHING_MM) + class restorer_smoothing { + const float val_; + public: + restorer_smoothing(const float temp_val) : val_(backlash.get_smoothing_mm()) { backlash.set_smoothing_mm(temp_val); } + ~restorer_smoothing() { backlash.set_smoothing_mm(val_); } + }; + + #define TEMPORARY_BACKLASH_SMOOTHING(value) restorer_smoothing restorer_tbsm(value) +#else + #define TEMPORARY_BACKLASH_SMOOTHING(value) +#endif + +inline void calibration_move() { + do_blocking_move_to((xyz_pos_t)current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); +} + +/** + * Move to the exact center above the calibration object + * + * m in - Measurement record + * uncertainty in - How far away from the object top to park + */ +inline void park_above_object(measurements_t &m, const float uncertainty) { + // Move to safe distance above calibration object + current_position.z = m.obj_center.z + dimensions.z / 2 + uncertainty; + calibration_move(); + + // Move to center of calibration object in XY + current_position = xy_pos_t(m.obj_center); + calibration_move(); +} + +#if HAS_MULTI_HOTEND + inline void set_nozzle(measurements_t &m, const uint8_t extruder) { + if (extruder != active_extruder) { + park_above_object(m, CALIBRATION_MEASUREMENT_UNKNOWN); + tool_change(extruder); + } + } +#endif + +#if HAS_HOTEND_OFFSET + + inline void normalize_hotend_offsets() { + LOOP_S_L_N(e, 1, HOTENDS) + hotend_offset[e] -= hotend_offset[0]; + hotend_offset[0].reset(); + } + +#endif + +#if !PIN_EXISTS(CALIBRATION) + #include "../../module/probe.h" +#endif + +inline bool read_calibration_pin() { + return ( + #if PIN_EXISTS(CALIBRATION) + READ(CALIBRATION_PIN) != CALIBRATION_PIN_INVERTING + #else + PROBE_TRIGGERED() + #endif + ); +} + +/** + * Move along axis in the specified dir until the probe value becomes stop_state, + * then return the axis value. + * + * axis in - Axis along which the measurement will take place + * dir in - Direction along that axis (-1 or 1) + * stop_state in - Move until probe pin becomes this value + * fast in - Fast vs. precise measurement + */ +float measuring_movement(const AxisEnum axis, const int dir, const bool stop_state, const bool fast) { + const float step = fast ? 0.25 : CALIBRATION_MEASUREMENT_RESOLUTION; + const feedRate_t mms = fast ? MMM_TO_MMS(CALIBRATION_FEEDRATE_FAST) : MMM_TO_MMS(CALIBRATION_FEEDRATE_SLOW); + const float limit = fast ? 50 : 5; + + destination = current_position; + for (float travel = 0; travel < limit; travel += step) { + destination[axis] += dir * step; + do_blocking_move_to((xyz_pos_t)destination, mms); + planner.synchronize(); + if (read_calibration_pin() == stop_state) break; + } + return destination[axis]; +} + +/** + * Move along axis until the probe is triggered. Move toolhead to its starting + * point and return the measured value. + * + * axis in - Axis along which the measurement will take place + * dir in - Direction along that axis (-1 or 1) + * stop_state in - Move until probe pin becomes this value + * backlash_ptr in/out - When not nullptr, measure and record axis backlash + * uncertainty in - If uncertainty is CALIBRATION_MEASUREMENT_UNKNOWN, do a fast probe. + */ +inline float measure(const AxisEnum axis, const int dir, const bool stop_state, float * const backlash_ptr, const float uncertainty) { + const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN; + + // Save the current position of the specified axis + const float start_pos = current_position[axis]; + + // Take a measurement. Only the specified axis will be affected. + const float measured_pos = measuring_movement(axis, dir, stop_state, fast); + + // Measure backlash + if (backlash_ptr && !fast) { + const float release_pos = measuring_movement(axis, -dir, !stop_state, fast); + *backlash_ptr = ABS(release_pos - measured_pos); + } + + // Move back to the starting position + destination = current_position; + destination[axis] = start_pos; + do_blocking_move_to((xyz_pos_t)destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); + return measured_pos; +} + +/** + * Probe one side of the calibration object + * + * m in/out - Measurement record, m.obj_center and m.obj_side will be updated. + * uncertainty in - How far away from the calibration object to begin probing + * side in - Side of probe where probe will occur + * probe_top_at_edge in - When probing sides, probe top of calibration object nearest edge + * to find out height of edge + */ +inline void probe_side(measurements_t &m, const float uncertainty, const side_t side, const bool probe_top_at_edge=false) { + const xyz_float_t dimensions = CALIBRATION_OBJECT_DIMENSIONS; + AxisEnum axis; + float dir = 1; + + park_above_object(m, uncertainty); + + #define _ACASE(N,A,B) case A: dir = -1; case B: axis = N##_AXIS; break + #define _PCASE(N) _ACASE(N, N##MINIMUM, N##MAXIMUM) + + switch (side) { + #if AXIS_CAN_CALIBRATE(X) + _ACASE(X, RIGHT, LEFT); + #endif + #if HAS_Y_AXIS && AXIS_CAN_CALIBRATE(Y) + _ACASE(Y, BACK, FRONT); + #endif + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + case TOP: { + const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty); + m.obj_center.z = measurement - dimensions.z / 2; + m.obj_side[TOP] = measurement; + return; + } + #endif + #if HAS_I_AXIS && AXIS_CAN_CALIBRATE(I) + _PCASE(I); + #endif + #if HAS_J_AXIS && AXIS_CAN_CALIBRATE(J) + _PCASE(J); + #endif + #if HAS_K_AXIS && AXIS_CAN_CALIBRATE(K) + _PCASE(K); + #endif + default: return; + } + + if (probe_top_at_edge) { + #if AXIS_CAN_CALIBRATE(Z) + // Probe top nearest the side we are probing + current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 - m.nozzle_outer_dimension[axis]); + calibration_move(); + m.obj_side[TOP] = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty); + m.obj_center.z = m.obj_side[TOP] - dimensions.z / 2; + #endif + } + + if ((AXIS_CAN_CALIBRATE(X) && axis == X_AXIS) || (AXIS_CAN_CALIBRATE(Y) && axis == Y_AXIS)) { + // Move to safe distance to the side of the calibration object + current_position[axis] = m.obj_center[axis] + (-dir) * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2 + uncertainty); + calibration_move(); + + // Plunge below the side of the calibration object and measure + current_position.z = m.obj_side[TOP] - (CALIBRATION_NOZZLE_TIP_HEIGHT) * 0.7f; + calibration_move(); + const float measurement = measure(axis, dir, true, &m.backlash[side], uncertainty); + m.obj_center[axis] = measurement + dir * (dimensions[axis] / 2 + m.nozzle_outer_dimension[axis] / 2); + m.obj_side[side] = measurement; + } +} + +/** + * Probe all sides of the calibration calibration object + * + * m in/out - Measurement record: center, backlash and error values be updated. + * uncertainty in - How far away from the calibration object to begin probing + */ +inline void probe_sides(measurements_t &m, const float uncertainty) { + #if ENABLED(CALIBRATION_MEASURE_AT_TOP_EDGES) + constexpr bool probe_top_at_edge = true; + #else + // Probing at the exact center only works if the center is flat. Probing on a washer + // or bolt will require probing the top near the side edges, away from the center. + constexpr bool probe_top_at_edge = false; + probe_side(m, uncertainty, TOP); + #endif + + TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_LEFT, probe_side(m, uncertainty, LEFT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_BACK, probe_side(m, uncertainty, BACK, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_IMIN, probe_side(m, uncertainty, IMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_IMAX, probe_side(m, uncertainty, IMAXIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_JMIN, probe_side(m, uncertainty, JMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_JMAX, probe_side(m, uncertainty, JMAXIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_KMIN, probe_side(m, uncertainty, KMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_KMAX, probe_side(m, uncertainty, KMAXIMUM, probe_top_at_edge)); + + // Compute the measured center of the calibration object. + TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2); + TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2); + TERN_(HAS_I_CENTER, m.obj_center.i = (m.obj_side[IMINIMUM] + m.obj_side[IMAXIMUM]) / 2); + TERN_(HAS_J_CENTER, m.obj_center.j = (m.obj_side[JMINIMUM] + m.obj_side[JMAXIMUM]) / 2); + TERN_(HAS_K_CENTER, m.obj_center.k = (m.obj_side[KMINIMUM] + m.obj_side[KMAXIMUM]) / 2); + + // Compute the outside diameter of the nozzle at the height + // at which it makes contact with the calibration object + TERN_(HAS_X_CENTER, m.nozzle_outer_dimension.x = m.obj_side[RIGHT] - m.obj_side[LEFT] - dimensions.x); + TERN_(HAS_Y_CENTER, m.nozzle_outer_dimension.y = m.obj_side[BACK] - m.obj_side[FRONT] - dimensions.y); + + park_above_object(m, uncertainty); + + // The difference between the known and the measured location + // of the calibration object is the positional error + NUM_AXIS_CODE( + m.pos_error.x = TERN0(HAS_X_CENTER, true_center.x - m.obj_center.x), + m.pos_error.y = TERN0(HAS_Y_CENTER, true_center.y - m.obj_center.y), + m.pos_error.z = true_center.z - m.obj_center.z, + m.pos_error.i = TERN0(HAS_I_CENTER, true_center.i - m.obj_center.i), + m.pos_error.j = TERN0(HAS_J_CENTER, true_center.j - m.obj_center.j), + m.pos_error.k = TERN0(HAS_K_CENTER, true_center.k - m.obj_center.k) + ); +} + +#if ENABLED(CALIBRATION_REPORTING) + inline void report_measured_faces(const measurements_t &m) { + SERIAL_ECHOLNPGM("Sides:"); + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM(" Top: ", m.obj_side[TOP]); + #endif + #if ENABLED(CALIBRATION_MEASURE_LEFT) + SERIAL_ECHOLNPGM(" Left: ", m.obj_side[LEFT]); + #endif + #if ENABLED(CALIBRATION_MEASURE_RIGHT) + SERIAL_ECHOLNPGM(" Right: ", m.obj_side[RIGHT]); + #endif + #if HAS_Y_AXIS + #if ENABLED(CALIBRATION_MEASURE_FRONT) + SERIAL_ECHOLNPGM(" Front: ", m.obj_side[FRONT]); + #endif + #if ENABLED(CALIBRATION_MEASURE_BACK) + SERIAL_ECHOLNPGM(" Back: ", m.obj_side[BACK]); + #endif + #endif + #if HAS_I_AXIS + #if ENABLED(CALIBRATION_MEASURE_IMIN) + SERIAL_ECHOLNPGM(" " STR_I_MIN ": ", m.obj_side[IMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_IMAX) + SERIAL_ECHOLNPGM(" " STR_I_MAX ": ", m.obj_side[IMAXIMUM]); + #endif + #endif + #if HAS_J_AXIS + #if ENABLED(CALIBRATION_MEASURE_JMIN) + SERIAL_ECHOLNPGM(" " STR_J_MIN ": ", m.obj_side[JMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_JMAX) + SERIAL_ECHOLNPGM(" " STR_J_MAX ": ", m.obj_side[JMAXIMUM]); + #endif + #endif + #if HAS_K_AXIS + #if ENABLED(CALIBRATION_MEASURE_KMIN) + SERIAL_ECHOLNPGM(" " STR_K_MIN ": ", m.obj_side[KMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_KMAX) + SERIAL_ECHOLNPGM(" " STR_K_MAX ": ", m.obj_side[KMAXIMUM]); + #endif + #endif + SERIAL_EOL(); + } + + inline void report_measured_center(const measurements_t &m) { + SERIAL_ECHOLNPGM("Center:"); + #if HAS_X_CENTER + SERIAL_ECHOLNPGM_P(SP_X_STR, m.obj_center.x); + #endif + #if HAS_Y_CENTER + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.obj_center.y); + #endif + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.obj_center.z); + #if HAS_I_CENTER + SERIAL_ECHOLNPGM_P(SP_I_STR, m.obj_center.i); + #endif + #if HAS_J_CENTER + SERIAL_ECHOLNPGM_P(SP_J_STR, m.obj_center.j); + #endif + #if HAS_K_CENTER + SERIAL_ECHOLNPGM_P(SP_K_STR, m.obj_center.k); + #endif + SERIAL_EOL(); + } + + inline void report_measured_backlash(const measurements_t &m) { + SERIAL_ECHOLNPGM("Backlash:"); + #if AXIS_CAN_CALIBRATE(X) + #if ENABLED(CALIBRATION_MEASURE_LEFT) + SERIAL_ECHOLNPGM(" Left: ", m.backlash[LEFT]); + #endif + #if ENABLED(CALIBRATION_MEASURE_RIGHT) + SERIAL_ECHOLNPGM(" Right: ", m.backlash[RIGHT]); + #endif + #endif + #if HAS_Y_AXIS && AXIS_CAN_CALIBRATE(Y) + #if ENABLED(CALIBRATION_MEASURE_FRONT) + SERIAL_ECHOLNPGM(" Front: ", m.backlash[FRONT]); + #endif + #if ENABLED(CALIBRATION_MEASURE_BACK) + SERIAL_ECHOLNPGM(" Back: ", m.backlash[BACK]); + #endif + #endif + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM(" Top: ", m.backlash[TOP]); + #endif + #if HAS_I_AXIS && AXIS_CAN_CALIBRATE(I) + #if ENABLED(CALIBRATION_MEASURE_IMIN) + SERIAL_ECHOLNPGM(" " STR_I_MIN ": ", m.backlash[IMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_IMAX) + SERIAL_ECHOLNPGM(" " STR_I_MAX ": ", m.backlash[IMAXIMUM]); + #endif + #endif + #if HAS_J_AXIS && AXIS_CAN_CALIBRATE(J) + #if ENABLED(CALIBRATION_MEASURE_JMIN) + SERIAL_ECHOLNPGM(" " STR_J_MIN ": ", m.backlash[JMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_JMAX) + SERIAL_ECHOLNPGM(" " STR_J_MAX ": ", m.backlash[JMAXIMUM]); + #endif + #endif + #if HAS_K_AXIS && AXIS_CAN_CALIBRATE(K) + #if ENABLED(CALIBRATION_MEASURE_KMIN) + SERIAL_ECHOLNPGM(" " STR_K_MIN ": ", m.backlash[KMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_KMAX) + SERIAL_ECHOLNPGM(" " STR_K_MAX ": ", m.backlash[KMAXIMUM]); + #endif + #endif + SERIAL_EOL(); + } + + inline void report_measured_positional_error(const measurements_t &m) { + SERIAL_CHAR('T'); + SERIAL_ECHO(active_extruder); + SERIAL_ECHOLNPGM(" Positional Error:"); + #if HAS_X_CENTER && AXIS_CAN_CALIBRATE(X) + SERIAL_ECHOLNPGM_P(SP_X_STR, m.pos_error.x); + #endif + #if HAS_Y_CENTER && AXIS_CAN_CALIBRATE(Y) + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.pos_error.y); + #endif + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.pos_error.z); + #endif + #if HAS_I_CENTER && AXIS_CAN_CALIBRATE(I) + SERIAL_ECHOLNPGM_P(SP_I_STR, m.pos_error.i); + #endif + #if HAS_J_CENTER && AXIS_CAN_CALIBRATE(J) + SERIAL_ECHOLNPGM_P(SP_J_STR, m.pos_error.j); + #endif + #if HAS_K_CENTER && AXIS_CAN_CALIBRATE(K) + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.pos_error.z); + #endif + SERIAL_EOL(); + } + + inline void report_measured_nozzle_dimensions(const measurements_t &m) { + SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:"); + #if HAS_X_CENTER + SERIAL_ECHOLNPGM_P(SP_X_STR, m.nozzle_outer_dimension.x); + #endif + #if HAS_Y_CENTER + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.nozzle_outer_dimension.y); + #endif + SERIAL_EOL(); + UNUSED(m); + } + + #if HAS_HOTEND_OFFSET + // + // This function requires normalize_hotend_offsets() to be called + // + inline void report_hotend_offsets() { + LOOP_S_L_N(e, 1, HOTENDS) + SERIAL_ECHOLNPGM_P(PSTR("T"), e, PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z); + } + #endif + +#endif // CALIBRATION_REPORTING + +/** + * Probe around the calibration object to measure backlash + * + * m in/out - Measurement record, updated with new readings + * uncertainty in - How far away from the object to begin probing + */ +inline void calibrate_backlash(measurements_t &m, const float uncertainty) { + // Backlash compensation should be off while measuring backlash + + { + // New scope for TEMPORARY_BACKLASH_CORRECTION + TEMPORARY_BACKLASH_CORRECTION(backlash.all_off); + TEMPORARY_BACKLASH_SMOOTHING(0.0f); + + probe_sides(m, uncertainty); + + #if ENABLED(BACKLASH_GCODE) + + #if HAS_X_CENTER + backlash.set_distance_mm(X_AXIS, (m.backlash[LEFT] + m.backlash[RIGHT]) / 2); + #elif ENABLED(CALIBRATION_MEASURE_LEFT) + backlash.set_distance_mm(X_AXIS, m.backlash[LEFT]); + #elif ENABLED(CALIBRATION_MEASURE_RIGHT) + backlash.set_distance_mm(X_AXIS, m.backlash[RIGHT]); + #endif + + #if HAS_Y_CENTER + backlash.set_distance_mm(Y_AXIS, (m.backlash[FRONT] + m.backlash[BACK]) / 2); + #elif ENABLED(CALIBRATION_MEASURE_FRONT) + backlash.set_distance_mm(Y_AXIS, m.backlash[FRONT]); + #elif ENABLED(CALIBRATION_MEASURE_BACK) + backlash.set_distance_mm(Y_AXIS, m.backlash[BACK]); + #endif + + TERN_(HAS_Z_AXIS, if (AXIS_CAN_CALIBRATE(Z)) backlash.set_distance_mm(Z_AXIS, m.backlash[TOP])); + + #if HAS_I_CENTER + backlash.set_distance_mm(I_AXIS, (m.backlash[IMINIMUM] + m.backlash[IMAXIMUM]) / 2); + #elif ENABLED(CALIBRATION_MEASURE_IMIN) + backlash.set_distance_mm(I_AXIS, m.backlash[IMINIMUM]); + #elif ENABLED(CALIBRATION_MEASURE_IMAX) + backlash.set_distance_mm(I_AXIS, m.backlash[IMAXIMUM]); + #endif + + #if HAS_J_CENTER + backlash.set_distance_mm(J_AXIS, (m.backlash[JMINIMUM] + m.backlash[JMAXIMUM]) / 2); + #elif ENABLED(CALIBRATION_MEASURE_JMIN) + backlash.set_distance_mm(J_AXIS, m.backlash[JMINIMUM]); + #elif ENABLED(CALIBRATION_MEASURE_JMAX) + backlash.set_distance_mm(J_AXIS, m.backlash[JMAXIMUM]); + #endif + + #if HAS_K_CENTER + backlash.set_distance_mm(K_AXIS, (m.backlash[KMINIMUM] + m.backlash[KMAXIMUM]) / 2); + #elif ENABLED(CALIBRATION_MEASURE_KMIN) + backlash.set_distance_mm(K_AXIS, m.backlash[KMINIMUM]); + #elif ENABLED(CALIBRATION_MEASURE_KMAX) + backlash.set_distance_mm(K_AXIS, m.backlash[KMAXIMUM]); + #endif + + #endif // BACKLASH_GCODE + } + + #if ENABLED(BACKLASH_GCODE) + // Turn on backlash compensation and move in all + // allowed directions to take up any backlash + { + // New scope for TEMPORARY_BACKLASH_CORRECTION + TEMPORARY_BACKLASH_CORRECTION(backlash.all_on); + TEMPORARY_BACKLASH_SMOOTHING(0.0f); + const xyz_float_t move = NUM_AXIS_ARRAY( + AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3, + AXIS_CAN_CALIBRATE(I) * 3, AXIS_CAN_CALIBRATE(J) * 3, AXIS_CAN_CALIBRATE(K) * 3 + ); + current_position += move; calibration_move(); + current_position -= move; calibration_move(); + } + #endif +} + +inline void update_measurements(measurements_t &m, const AxisEnum axis) { + current_position[axis] += m.pos_error[axis]; + m.obj_center[axis] = true_center[axis]; + m.pos_error[axis] = 0; +} + +/** + * Probe around the calibration object. Adjust the position and toolhead offset + * using the deviation from the known position of the calibration object. + * + * m in/out - Measurement record, updated with new readings + * uncertainty in - How far away from the object to begin probing + * extruder in - What extruder to probe + * + * Prerequisites: + * - Call calibrate_backlash() beforehand for best accuracy + */ +inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const uint8_t extruder) { + TEMPORARY_BACKLASH_CORRECTION(backlash.all_on); + TEMPORARY_BACKLASH_SMOOTHING(0.0f); + + TERN(HAS_MULTI_HOTEND, set_nozzle(m, extruder), UNUSED(extruder)); + + probe_sides(m, uncertainty); + + // Adjust the hotend offset + #if HAS_HOTEND_OFFSET + if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) hotend_offset[extruder].x += m.pos_error.x; + if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) hotend_offset[extruder].y += m.pos_error.y; + if (AXIS_CAN_CALIBRATE(Z)) hotend_offset[extruder].z += m.pos_error.z; + normalize_hotend_offsets(); + #endif + + // Correct for positional error, so the object + // is at the known actual spot + planner.synchronize(); + if (ENABLED(HAS_X_CENTER) && AXIS_CAN_CALIBRATE(X)) update_measurements(m, X_AXIS); + if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) update_measurements(m, Y_AXIS); + if (AXIS_CAN_CALIBRATE(Z)) update_measurements(m, Z_AXIS); + + TERN_(HAS_I_CENTER, update_measurements(m, I_AXIS)); + TERN_(HAS_J_CENTER, update_measurements(m, J_AXIS)); + TERN_(HAS_K_CENTER, update_measurements(m, K_AXIS)); + + sync_plan_position(); +} + +/** + * Probe around the calibration object for all toolheads, adjusting the coordinate + * system for the first nozzle and the nozzle offset for subsequent nozzles. + * + * m in/out - Measurement record, updated with new readings + * uncertainty in - How far away from the object to begin probing + */ +inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) { + TEMPORARY_BACKLASH_CORRECTION(backlash.all_on); + TEMPORARY_BACKLASH_SMOOTHING(0.0f); + + HOTEND_LOOP() calibrate_toolhead(m, uncertainty, e); + + TERN_(HAS_HOTEND_OFFSET, normalize_hotend_offsets()); + + TERN_(HAS_MULTI_HOTEND, set_nozzle(m, 0)); +} + +/** + * Perform a full auto-calibration routine: + * + * 1) For each nozzle, touch top and sides of object to determine object position and + * nozzle offsets. Do a fast but rough search over a wider area. + * 2) With the first nozzle, touch top and sides of object to determine backlash values + * for all axes (if BACKLASH_GCODE is enabled) + * 3) For each nozzle, touch top and sides of object slowly to determine precise + * position of object. Adjust coordinate system and nozzle offsets so probed object + * location corresponds to known object location with a high degree of precision. + */ +inline void calibrate_all() { + measurements_t m; + + TERN_(HAS_HOTEND_OFFSET, reset_hotend_offsets()); + + TEMPORARY_BACKLASH_CORRECTION(backlash.all_on); + TEMPORARY_BACKLASH_SMOOTHING(0.0f); + + // Do a fast and rough calibration of the toolheads + calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNKNOWN); + + TERN_(BACKLASH_GCODE, calibrate_backlash(m, CALIBRATION_MEASUREMENT_UNCERTAIN)); + + // Cycle the toolheads so the servos settle into their "natural" positions + #if HAS_MULTI_HOTEND + HOTEND_LOOP() set_nozzle(m, e); + #endif + + // Do a slow and precise calibration of the toolheads + calibrate_all_toolheads(m, CALIBRATION_MEASUREMENT_UNCERTAIN); + + current_position.x = X_CENTER; + calibration_move(); // Park nozzle away from calibration object +} + +/** + * G425: Perform calibration with calibration object. + * + * B - Perform calibration of backlash only. + * T - Perform calibration of toolhead only. + * V - Probe object and print position, error, backlash and hotend offset. + * U - Uncertainty, how far to start probe away from the object (mm) + * + * no args - Perform entire calibration sequence (backlash + position on all toolheads) + */ +void GcodeSuite::G425() { + + #ifdef CALIBRATION_SCRIPT_PRE + process_subcommands_now(F(CALIBRATION_SCRIPT_PRE)); + #endif + + if (homing_needed_error()) return; + + TEMPORARY_BED_LEVELING_STATE(false); + SET_SOFT_ENDSTOP_LOOSE(true); + + measurements_t m; + const float uncertainty = parser.floatval('U', CALIBRATION_MEASUREMENT_UNCERTAIN); + + if (parser.seen_test('B')) + calibrate_backlash(m, uncertainty); + else if (parser.seen_test('T')) + calibrate_toolhead(m, uncertainty, parser.intval('T', active_extruder)); + #if ENABLED(CALIBRATION_REPORTING) + else if (parser.seen('V')) { + probe_sides(m, uncertainty); + SERIAL_EOL(); + report_measured_faces(m); + report_measured_center(m); + report_measured_backlash(m); + report_measured_nozzle_dimensions(m); + report_measured_positional_error(m); + #if HAS_HOTEND_OFFSET + normalize_hotend_offsets(); + report_hotend_offsets(); + #endif + } + #endif + else + calibrate_all(); + + SET_SOFT_ENDSTOP_LOOSE(false); + + #ifdef CALIBRATION_SCRIPT_POST + process_subcommands_now(F(CALIBRATION_SCRIPT_POST)); + #endif +} + +#endif // CALIBRATION_GCODE diff --git a/src/gcode/calibrate/G76_M871.cpp b/src/gcode/calibrate/G76_M871.cpp new file mode 100644 index 0000000..c484d4f --- /dev/null +++ b/src/gcode/calibrate/G76_M871.cpp @@ -0,0 +1,339 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +/** + * G76_M871.cpp - Temperature calibration/compensation for z-probing + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_PTC + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/planner.h" +#include "../../module/probe.h" +#include "../../feature/bedlevel/bedlevel.h" +#include "../../module/temperature.h" +#include "../../module/probe.h" +#include "../../feature/probe_temp_comp.h" +#include "../../lcd/marlinui.h" + +/** + * G76: calibrate probe and/or bed temperature offsets + * Notes: + * - When calibrating probe, bed temperature is held constant. + * Compensation values are deltas to first probe measurement at probe temp. = 30°C. + * - When calibrating bed, probe temperature is held constant. + * Compensation values are deltas to first probe measurement at bed temp. = 60°C. + * - The hotend will not be heated at any time. + * - On my Průša MK3S clone I put a piece of paper between the probe and the hotend + * so the hotend fan would not cool my probe constantly. Alternatively you could just + * make sure the fan is not running while running the calibration process. + * + * Probe calibration: + * - Moves probe to cooldown point. + * - Heats up bed to 100°C. + * - Moves probe to probing point (1mm above heatbed). + * - Waits until probe reaches target temperature (30°C). + * - Does a z-probing (=base value) and increases target temperature by 5°C. + * - Waits until probe reaches increased target temperature. + * - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C. + * - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further). + * - Compensation values of higher temperatures will be extrapolated (using linear regression first). + * While this is not exact by any means it is still better than simply using the last compensation value. + * + * Bed calibration: + * - Moves probe to cooldown point. + * - Heats up bed to 60°C. + * - Moves probe to probing point (1mm above heatbed). + * - Waits until probe reaches target temperature (30°C). + * - Does a z-probing (=base value) and increases bed temperature by 5°C. + * - Moves probe to cooldown point. + * - Waits until probe is below 30°C and bed has reached target temperature. + * - Moves probe to probing point and waits until it reaches target temperature (30°C). + * - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C. + * - Repeats last four points until max. bed temperature reached (110°C) or timeout. + * - Compensation values of higher temperatures will be extrapolated (using linear regression first). + * While this is not exact by any means it is still better than simply using the last compensation value. + * + * G76 [B | P] + * - no flag - Both calibration procedures will be run. + * - `B` - Run bed temperature calibration. + * - `P` - Run probe temperature calibration. + */ + +#if BOTH(PTC_PROBE, PTC_BED) + + static void say_waiting_for() { SERIAL_ECHOPGM("Waiting for "); } + static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); } + static void say_successfully_calibrated() { SERIAL_ECHOPGM("Successfully calibrated"); } + static void say_failed_to_calibrate() { SERIAL_ECHOPGM("!Failed to calibrate"); } + + void GcodeSuite::G76() { + auto report_temps = [](millis_t &ntr, millis_t timeout=0) { + idle_no_sleep(); + const millis_t ms = millis(); + if (ELAPSED(ms, ntr)) { + ntr = ms + 1000; + thermalManager.print_heater_states(active_extruder); + } + return (timeout && ELAPSED(ms, timeout)); + }; + + auto wait_for_temps = [&](const celsius_t tb, const celsius_t tp, millis_t &ntr, const millis_t timeout=0) { + say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature."); + while (thermalManager.wholeDegBed() != tb || thermalManager.wholeDegProbe() > tp) + if (report_temps(ntr, timeout)) return true; + return false; + }; + + auto g76_probe = [](const TempSensorID sid, celsius_t &targ, const xy_pos_t &nozpos) { + do_z_clearance(5.0); // Raise nozzle before probing + ptc.set_enabled(false); + const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false); // verbose=0, probe_relative=false + ptc.set_enabled(true); + if (isnan(measured_z)) + SERIAL_ECHOLNPGM("!Received NAN. Aborting."); + else { + SERIAL_ECHOLNPAIR_F("Measured: ", measured_z); + if (targ == ProbeTempComp::cali_info[sid].start_temp) + ptc.prepare_new_calibration(measured_z); + else + ptc.push_back_new_measurement(sid, measured_z); + targ += ProbeTempComp::cali_info[sid].temp_resolution; + } + return measured_z; + }; + + #if ENABLED(BLTOUCH) + // Make sure any BLTouch error condition is cleared + bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); + set_bltouch_deployed(false); + #endif + + bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P'); + if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true; + + // Synchronize with planner + planner.synchronize(); + + #ifndef PTC_PROBE_HEATING_OFFSET + #define PTC_PROBE_HEATING_OFFSET 0 + #endif + const xyz_pos_t parkpos = PTC_PARK_POS, + probe_pos_xyz = xyz_pos_t(PTC_PROBE_POS) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }), + noz_pos_xyz = probe_pos_xyz - probe.offset_xy; // Nozzle position based on probe position + + if (do_bed_cal || do_probe_cal) { + // Ensure park position is reachable + bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop); + if (!reachable) + SERIAL_ECHOLNPGM("!Park"); + else { + // Ensure probe position is reachable + reachable = probe.can_reach(probe_pos_xyz); + if (!reachable) SERIAL_ECHOLNPGM("!Probe"); + } + + if (!reachable) { + SERIAL_ECHOLNPGM(" position unreachable - aborting."); + return; + } + + process_subcommands_now(FPSTR(G28_STR)); + } + + remember_feedrate_scaling_off(); + + /****************************************** + * Calibrate bed temperature offsets + ******************************************/ + + // Report temperatures every second and handle heating timeouts + millis_t next_temp_report = millis() + 1000; + + auto report_targets = [&](const celsius_t tb, const celsius_t tp) { + SERIAL_ECHOLNPGM("Target Bed:", tb, " Probe:", tp); + }; + + if (do_bed_cal) { + + celsius_t target_bed = PTC_BED_START, + target_probe = PTC_PROBE_TEMP; + + say_waiting_for(); SERIAL_ECHOLNPGM(" cooling."); + while (thermalManager.wholeDegBed() > target_bed || thermalManager.wholeDegProbe() > target_probe) + report_temps(next_temp_report); + + // Disable leveling so it won't mess with us + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + + for (uint8_t idx = 0; idx <= PTC_BED_COUNT; idx++) { + thermalManager.setTargetBed(target_bed); + + report_targets(target_bed, target_probe); + + // Park nozzle + do_blocking_move_to(parkpos); + + // Wait for heatbed to reach target temp and probe to cool below target temp + if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) { + SERIAL_ECHOLNPGM("!Bed heating timeout."); + break; + } + + // Move the nozzle to the probing point and wait for the probe to reach target temp + do_blocking_move_to(noz_pos_xyz); + say_waiting_for_probe_heating(); + SERIAL_EOL(); + while (thermalManager.wholeDegProbe() < target_probe) + report_temps(next_temp_report); + + const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz); + if (isnan(measured_z) || target_bed > (BED_MAX_TARGET)) break; + } + + SERIAL_ECHOLNPGM("Retrieved measurements: ", ptc.get_index()); + if (ptc.finish_calibration(TSI_BED)) { + say_successfully_calibrated(); + SERIAL_ECHOLNPGM(" bed."); + } + else { + say_failed_to_calibrate(); + SERIAL_ECHOLNPGM(" bed. Values reset."); + } + + // Cleanup + thermalManager.setTargetBed(0); + TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); + } // do_bed_cal + + /******************************************** + * Calibrate probe temperature offsets + ********************************************/ + + if (do_probe_cal) { + + // Park nozzle + do_blocking_move_to(parkpos); + + // Initialize temperatures + const celsius_t target_bed = BED_MAX_TARGET; + thermalManager.setTargetBed(target_bed); + + celsius_t target_probe = PTC_PROBE_START; + + report_targets(target_bed, target_probe); + + // Wait for heatbed to reach target temp and probe to cool below target temp + wait_for_temps(target_bed, target_probe, next_temp_report); + + // Disable leveling so it won't mess with us + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + + bool timeout = false; + for (uint8_t idx = 0; idx <= PTC_PROBE_COUNT; idx++) { + // Move probe to probing point and wait for it to reach target temperature + do_blocking_move_to(noz_pos_xyz); + + say_waiting_for_probe_heating(); + SERIAL_ECHOLNPGM(" Bed:", target_bed, " Probe:", target_probe); + const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL); + while (thermalManager.degProbe() < target_probe) { + if (report_temps(next_temp_report, probe_timeout_ms)) { + SERIAL_ECHOLNPGM("!Probe heating timed out."); + timeout = true; + break; + } + } + if (timeout) break; + + const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz); + if (isnan(measured_z)) break; + } + + SERIAL_ECHOLNPGM("Retrieved measurements: ", ptc.get_index()); + if (ptc.finish_calibration(TSI_PROBE)) + say_successfully_calibrated(); + else + say_failed_to_calibrate(); + SERIAL_ECHOLNPGM(" probe."); + + // Cleanup + thermalManager.setTargetBed(0); + TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); + + SERIAL_ECHOLNPGM("Final compensation values:"); + ptc.print_offsets(); + } // do_probe_cal + + restore_feedrate_and_scaling(); + } + +#endif // PTC_PROBE && PTC_BED + +/** + * M871: Report / reset temperature compensation offsets. + * Note: This does not affect values in EEPROM until M500. + * + * M871 [ R | B | P | E ] + * + * No Parameters - Print current offset values. + * + * Select only one of these flags: + * R - Reset all offsets to zero (i.e., disable compensation). + * B - Manually set offset for bed + * P - Manually set offset for probe + * E - Manually set offset for extruder + * + * With B, P, or E: + * I[index] - Index in the array + * V[value] - Adjustment in µm + */ +void GcodeSuite::M871() { + + if (parser.seen('R')) { + // Reset z-probe offsets to factory defaults + ptc.clear_all_offsets(); + SERIAL_ECHOLNPGM("Offsets reset to default."); + } + else if (parser.seen("BPE")) { + if (!parser.seenval('V')) return; + const int16_t offset_val = parser.value_int(); + if (!parser.seenval('I')) return; + const int16_t idx = parser.value_int(); + const TempSensorID mod = TERN_(PTC_BED, parser.seen_test('B') ? TSI_BED :) + TERN_(PTC_HOTEND, parser.seen_test('E') ? TSI_EXT :) + TERN_(PTC_PROBE, parser.seen_test('P') ? TSI_PROBE :) TSI_COUNT; + if (mod == TSI_COUNT) + SERIAL_ECHOLNPGM("!Invalid sensor."); + else if (idx > 0 && ptc.set_offset(mod, idx - 1, offset_val)) + SERIAL_ECHOLNPGM("Set value: ", offset_val); + else + SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant)."); + } + else // Print current Z-probe adjustments. Note: Values in EEPROM might differ. + ptc.print_offsets(); +} + +#endif // HAS_PTC diff --git a/src/gcode/calibrate/M100.cpp b/src/gcode/calibrate/M100.cpp new file mode 100644 index 0000000..338392b --- /dev/null +++ b/src/gcode/calibrate/M100.cpp @@ -0,0 +1,373 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(M100_FREE_MEMORY_WATCHER) + +#include "../gcode.h" +#include "../queue.h" +#include "../../libs/hex_print.h" + +#include "../../MarlinCore.h" // for idle() + +/** + * M100 Free Memory Watcher + * + * This code watches the free memory block between the bottom of the heap and the top of the stack. + * This memory block is initialized and watched via the M100 command. + * + * M100 I Initializes the free memory block and prints vitals statistics about the area + * + * M100 F Identifies how much of the free memory block remains free and unused. It also + * detects and reports any corruption within the free memory block that may have + * happened due to errant firmware. + * + * M100 D Does a hex display of the free memory block along with a flag for any errant + * data that does not match the expected value. + * + * M100 C x Corrupts x locations within the free memory block. This is useful to check the + * correctness of the M100 F and M100 D commands. + * + * Also, there are two support functions that can be called from a developer's C code. + * + * uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start); + * void M100_dump_routine(FSTR_P const title, const char * const start, const uintptr_t size); + * + * Initial version by Roxy-3D + */ +#define M100_FREE_MEMORY_DUMPER // Enable for the `M100 D` Dump sub-command +#define M100_FREE_MEMORY_CORRUPTOR // Enable for the `M100 C` Corrupt sub-command + +#define TEST_BYTE ((char) 0xE5) + +#if EITHER(__AVR__, IS_32BIT_TEENSY) + + extern char __bss_end; + char *end_bss = &__bss_end, + *free_memory_start = end_bss, *free_memory_end = 0, + *stacklimit = 0, *heaplimit = 0; + + #define MEMORY_END_CORRECTION 0 + +#elif defined(TARGET_LPC1768) + + extern char __bss_end__, __StackLimit, __HeapLimit; + + char *end_bss = &__bss_end__, + *stacklimit = &__StackLimit, + *heaplimit = &__HeapLimit; + + #define MEMORY_END_CORRECTION 0x200 + + char *free_memory_start = heaplimit, + *free_memory_end = stacklimit - MEMORY_END_CORRECTION; + +#elif defined(__SAM3X8E__) + + extern char _ebss; + + char *end_bss = &_ebss, + *free_memory_start = end_bss, + *free_memory_end = 0, + *stacklimit = 0, + *heaplimit = 0; + + #define MEMORY_END_CORRECTION 0x10000 // need to stay well below 0x20080000 or M100 F crashes + +#elif defined(__SAMD51__) + + extern unsigned int __bss_end__, __StackLimit, __HeapLimit; + extern "C" void * _sbrk(int incr); + + void *end_bss = &__bss_end__, + *stacklimit = &__StackLimit, + *heaplimit = &__HeapLimit; + + #define MEMORY_END_CORRECTION 0x400 + + char *free_memory_start = (char *)_sbrk(0) + 0x200, // Leave some heap space + *free_memory_end = (char *)stacklimit - MEMORY_END_CORRECTION; + +#else + #error "M100 - unsupported CPU" +#endif + +// +// Utility functions +// + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wreturn-local-addr" + +// Location of a variable in its stack frame. +// The returned address will be above the stack (after it returns). +char *top_of_stack() { + char x; + return &x + 1; // x is pulled on return; +} + +#pragma GCC diagnostic pop + +// Count the number of test bytes at the specified location. +inline int32_t count_test_bytes(const char * const start_free_memory) { + for (uint32_t i = 0; i < 32000; i++) + if (char(start_free_memory[i]) != TEST_BYTE) + return i - 1; + + return -1; +} + +// +// M100 sub-commands +// + +#if ENABLED(M100_FREE_MEMORY_DUMPER) + /** + * M100 D + * Dump the free memory block from brkval to the stack pointer. + * malloc() eats memory from the start of the block and the stack grows + * up from the bottom of the block. Solid test bytes indicate nothing has + * used that memory yet. There should not be anything but test bytes within + * the block. If so, it may indicate memory corruption due to a bad pointer. + * Unexpected bytes are flagged in the right column. + */ + void dump_free_memory(char *start_free_memory, char *end_free_memory) { + // + // Start and end the dump on a nice 16 byte boundary + // (even though the values are not 16-byte aligned). + // + start_free_memory = (char*)(uintptr_t(uint32_t(start_free_memory) & ~0xFUL)); // Align to 16-byte boundary + end_free_memory = (char*)(uintptr_t(uint32_t(end_free_memory) | 0xFUL)); // Align end_free_memory to the 15th byte (at or above end_free_memory) + + // Dump command main loop + while (start_free_memory < end_free_memory) { + print_hex_address(start_free_memory); // Print the address + SERIAL_CHAR(':'); + LOOP_L_N(i, 16) { // and 16 data bytes + if (i == 8) SERIAL_CHAR('-'); + print_hex_byte(start_free_memory[i]); + SERIAL_CHAR(' '); + } + serial_delay(25); + SERIAL_CHAR('|'); // Point out non test bytes + LOOP_L_N(i, 16) { + char ccc = (char)start_free_memory[i]; // cast to char before automatically casting to char on assignment, in case the compiler is broken + ccc = (ccc == TEST_BYTE) ? ' ' : '?'; + SERIAL_CHAR(ccc); + } + SERIAL_EOL(); + start_free_memory += 16; + serial_delay(25); + idle(); + } + } + + void M100_dump_routine(FSTR_P const title, const char * const start, const uintptr_t size) { + SERIAL_ECHOLNF(title); + // + // Round the start and end locations to produce full lines of output + // + const char * const end = start + size - 1; + dump_free_memory( + (char*)(uintptr_t(uint32_t(start) & ~0xFUL)), // Align to 16-byte boundary + (char*)(uintptr_t(uint32_t(end) | 0xFUL)) // Align end_free_memory to the 15th byte (at or above end_free_memory) + ); + } + +#endif // M100_FREE_MEMORY_DUMPER + +inline int check_for_free_memory_corruption(FSTR_P const title) { + SERIAL_ECHOF(title); + + char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end; + int n = end_free_memory - start_free_memory; + + SERIAL_ECHOLNPGM("\nfmc() n=", n, + "\nfree_memory_start=", hex_address(free_memory_start), + " end=", hex_address(end_free_memory)); + + if (end_free_memory < start_free_memory) { + SERIAL_ECHOPGM(" end_free_memory < Heap "); + //SET_INPUT_PULLUP(63); // if the developer has a switch wired up to their controller board + //safe_delay(5); // this code can be enabled to pause the display as soon as the + //while ( READ(63)) // malfunction is detected. It is currently defaulting to a switch + // idle(); // being on pin-63 which is unassigend and available on most controller + //safe_delay(20); // boards. + //while ( !READ(63)) + // idle(); + serial_delay(20); + #if ENABLED(M100_FREE_MEMORY_DUMPER) + M100_dump_routine(F(" Memory corruption detected with end_free_memory 8) { + //SERIAL_ECHOPGM("Found ", j); + //SERIAL_ECHOLNPGM(" bytes free at ", hex_address(start_free_memory + i)); + i += j; + block_cnt++; + SERIAL_ECHOLNPGM(" (", block_cnt, ") found=", j); + } + } + } + SERIAL_ECHOPGM(" block_found=", block_cnt); + + if (block_cnt != 1) + SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area."); + + if (block_cnt == 0) // Make sure the special case of no free blocks shows up as an + block_cnt = -1; // error to the calling code! + + SERIAL_ECHOPGM(" return="); + if (block_cnt == 1) { + SERIAL_CHAR('0'); // If the block_cnt is 1, nothing has broken up the free memory + SERIAL_EOL(); // area and it is appropriate to say 'no corruption'. + return 0; + } + SERIAL_ECHOLNPGM("true"); + return block_cnt; +} + +/** + * M100 F + * Return the number of free bytes in the memory pool, + * with other vital statistics defining the pool. + */ +inline void free_memory_pool_report(char * const start_free_memory, const int32_t size) { + int32_t max_cnt = -1, block_cnt = 0; + char *max_addr = nullptr; + // Find the longest block of test bytes in the buffer + for (int32_t i = 0; i < size; i++) { + char *addr = start_free_memory + i; + if (*addr == TEST_BYTE) { + const int32_t j = count_test_bytes(addr); + if (j > 8) { + SERIAL_ECHOLNPGM("Found ", j, " bytes free at ", hex_address(addr)); + if (j > max_cnt) { + max_cnt = j; + max_addr = addr; + } + i += j; + block_cnt++; + } + } + } + if (block_cnt > 1) SERIAL_ECHOLNPGM( + "\nMemory Corruption detected in free memory area." + "\nLargest free block is ", max_cnt, " bytes at ", hex_address(max_addr) + ); + SERIAL_ECHOLNPGM("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(F("M100 F "))); +} + +#if ENABLED(M100_FREE_MEMORY_CORRUPTOR) + /** + * M100 C + * Corrupt locations in the free memory pool and report the corrupt addresses. + * This is useful to check the correctness of the M100 D and the M100 F commands. + */ + inline void corrupt_free_memory(char *start_free_memory, const uintptr_t size) { + start_free_memory += 8; + const uint32_t near_top = top_of_stack() - start_free_memory - 250, // -250 to avoid interrupt activity that's altered the stack. + j = near_top / (size + 1); + + SERIAL_ECHOLNPGM("Corrupting free memory block."); + for (uint32_t i = 1; i <= size; i++) { + char * const addr = start_free_memory + i * j; + *addr = i; + SERIAL_ECHOPGM("\nCorrupting address: ", hex_address(addr)); + } + SERIAL_EOL(); + } +#endif // M100_FREE_MEMORY_CORRUPTOR + +/** + * M100 I + * Init memory for the M100 tests. (Automatically applied on the first M100.) + */ +inline void init_free_memory(char *start_free_memory, int32_t size) { + SERIAL_ECHOLNPGM("Initializing free memory block.\n\n"); + + size -= 250; // -250 to avoid interrupt activity that's altered the stack. + if (size < 0) { + SERIAL_ECHOLNPGM("Unable to initialize.\n"); + return; + } + + start_free_memory += 8; // move a few bytes away from the heap just because we + // don't want to be altering memory that close to it. + memset(start_free_memory, TEST_BYTE, size); + + SERIAL_ECHO(size); + SERIAL_ECHOLNPGM(" bytes of memory initialized.\n"); + + for (int32_t i = 0; i < size; i++) { + if (start_free_memory[i] != TEST_BYTE) { + SERIAL_ECHOPGM("? address : ", hex_address(start_free_memory + i)); + SERIAL_ECHOLNPGM("=", hex_byte(start_free_memory[i])); + SERIAL_EOL(); + } + } +} + +/** + * M100: Free Memory Check + */ +void GcodeSuite::M100() { + char *sp = top_of_stack(); + if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION; + SERIAL_ECHOPGM("\nbss_end : ", hex_address(end_bss)); + if (heaplimit) SERIAL_ECHOPGM("\n__heaplimit : ", hex_address(heaplimit)); + SERIAL_ECHOPGM("\nfree_memory_start : ", hex_address(free_memory_start)); + if (stacklimit) SERIAL_ECHOPGM("\n__stacklimit : ", hex_address(stacklimit)); + SERIAL_ECHOPGM("\nfree_memory_end : ", hex_address(free_memory_end)); + if (MEMORY_END_CORRECTION) + SERIAL_ECHOPGM("\nMEMORY_END_CORRECTION : ", MEMORY_END_CORRECTION); + SERIAL_ECHOLNPGM("\nStack Pointer : ", hex_address(sp)); + + // Always init on the first invocation of M100 + static bool m100_not_initialized = true; + if (m100_not_initialized || parser.seen('I')) { + m100_not_initialized = false; + init_free_memory(free_memory_start, free_memory_end - free_memory_start); + } + + #if ENABLED(M100_FREE_MEMORY_DUMPER) + if (parser.seen('D')) + return dump_free_memory(free_memory_start, free_memory_end); + #endif + + if (parser.seen('F')) + return free_memory_pool_report(free_memory_start, free_memory_end - free_memory_start); + + #if ENABLED(M100_FREE_MEMORY_CORRUPTOR) + if (parser.seen('C')) + return corrupt_free_memory(free_memory_start, parser.value_int()); + #endif +} + +#endif // M100_FREE_MEMORY_WATCHER diff --git a/src/gcode/calibrate/M12.cpp b/src/gcode/calibrate/M12.cpp new file mode 100644 index 0000000..191ff22 --- /dev/null +++ b/src/gcode/calibrate/M12.cpp @@ -0,0 +1,40 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER) + +#include "../gcode.h" +#include "../../module/planner.h" +#include "../../feature/closedloop.h" + +void GcodeSuite::M12() { + + planner.synchronize(); + + if (parser.seenval('S')) + closedloop.set(parser.value_int()); // Force a CLC set + +} + +#endif diff --git a/src/gcode/calibrate/M425.cpp b/src/gcode/calibrate/M425.cpp new file mode 100644 index 0000000..ba262c5 --- /dev/null +++ b/src/gcode/calibrate/M425.cpp @@ -0,0 +1,125 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(BACKLASH_GCODE) + +#include "../../feature/backlash.h" +#include "../../module/planner.h" + +#include "../gcode.h" + +/** + * M425: Enable and tune backlash correction. + * + * F Enable/disable/fade-out backlash correction (0.0 to 1.0) + * S Distance over which backlash correction is spread + * X Set the backlash distance on X (0 to disable) + * Y ... on Y + * Z ... on Z + * X If a backlash measurement was done on X, copy that value + * Y ... on Y + * Z ... on Z + * + * Type M425 without any arguments to show active values. + */ +void GcodeSuite::M425() { + bool noArgs = true; + + auto axis_can_calibrate = [](const uint8_t a) { + #define _CAN_CASE(N) case N##_AXIS: return AXIS_CAN_CALIBRATE(N); + switch (a) { + default: return false; + MAIN_AXIS_MAP(_CAN_CASE) + } + }; + + LOOP_NUM_AXES(a) { + if (axis_can_calibrate(a) && parser.seen(AXIS_CHAR(a))) { + planner.synchronize(); + backlash.set_distance_mm((AxisEnum)a, parser.has_value() ? parser.value_axis_units((AxisEnum)a) : backlash.get_measurement((AxisEnum)a)); + noArgs = false; + } + } + + if (parser.seen('F')) { + planner.synchronize(); + backlash.set_correction(parser.value_float()); + noArgs = false; + } + + #ifdef BACKLASH_SMOOTHING_MM + if (parser.seen('S')) { + planner.synchronize(); + backlash.set_smoothing_mm(parser.value_linear_units()); + noArgs = false; + } + #endif + + if (noArgs) { + SERIAL_ECHOPGM("Backlash Correction "); + if (!backlash.get_correction_uint8()) SERIAL_ECHOPGM("in"); + SERIAL_ECHOLNPGM("active:"); + SERIAL_ECHOLNPGM(" Correction Amount/Fade-out: F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)"); + SERIAL_ECHOPGM(" Backlash Distance (mm): "); + LOOP_NUM_AXES(a) if (axis_can_calibrate(a)) { + SERIAL_ECHOLNPGM_P((PGM_P)pgm_read_ptr(&SP_AXIS_STR[a]), backlash.get_distance_mm((AxisEnum)a)); + } + + #ifdef BACKLASH_SMOOTHING_MM + SERIAL_ECHOLNPGM(" Smoothing (mm): S", backlash.get_smoothing_mm()); + #endif + + #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING) + SERIAL_ECHOPGM(" Average measured backlash (mm):"); + if (backlash.has_any_measurement()) { + LOOP_NUM_AXES(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) { + SERIAL_ECHOPGM_P((PGM_P)pgm_read_ptr(&SP_AXIS_STR[a]), backlash.get_measurement((AxisEnum)a)); + } + } + else + SERIAL_ECHOPGM(" (Not yet measured)"); + SERIAL_EOL(); + #endif + } +} + +void GcodeSuite::M425_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_BACKLASH_COMPENSATION)); + SERIAL_ECHOLNPGM_P( + PSTR(" M425 F"), backlash.get_correction() + #ifdef BACKLASH_SMOOTHING_MM + , PSTR(" S"), LINEAR_UNIT(backlash.get_smoothing_mm()) + #endif + , LIST_N(DOUBLE(NUM_AXES), + SP_X_STR, LINEAR_UNIT(backlash.get_distance_mm(X_AXIS)), + SP_Y_STR, LINEAR_UNIT(backlash.get_distance_mm(Y_AXIS)), + SP_Z_STR, LINEAR_UNIT(backlash.get_distance_mm(Z_AXIS)), + SP_I_STR, LINEAR_UNIT(backlash.get_distance_mm(I_AXIS)), + SP_J_STR, LINEAR_UNIT(backlash.get_distance_mm(J_AXIS)), + SP_K_STR, LINEAR_UNIT(backlash.get_distance_mm(K_AXIS)) + ) + ); +} + +#endif // BACKLASH_GCODE diff --git a/src/gcode/calibrate/M48.cpp b/src/gcode/calibrate/M48.cpp new file mode 100644 index 0000000..8b6ea0b --- /dev/null +++ b/src/gcode/calibrate/M48.cpp @@ -0,0 +1,285 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST) + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/probe.h" +#include "../../lcd/marlinui.h" + +#include "../../feature/bedlevel/bedlevel.h" + +#if HAS_LEVELING + #include "../../module/planner.h" +#endif + +#if HAS_PTC + #include "../../feature/probe_temp_comp.h" +#endif + +/** + * M48: Z probe repeatability measurement function. + * + * Usage: + * M48 + * P = Number of sampled points (4-50, default 10) + * X = Sample X position + * Y = Sample Y position + * V = Verbose level (0-4, default=1) + * E = Engage Z probe for each reading + * L = Number of legs of movement before probe + * S = Schizoid (Or Star if you prefer) + * C = Enable probe temperature compensation (0 or 1, default 1) + * + * This function requires the machine to be homed before invocation. + */ + +void GcodeSuite::M48() { + + if (homing_needed_error()) return; + + const int8_t verbose_level = parser.byteval('V', 1); + if (!WITHIN(verbose_level, 0, 4)) { + SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4)."); + return; + } + + if (verbose_level > 0) + SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test"); + + const int8_t n_samples = parser.byteval('P', 10); + if (!WITHIN(n_samples, 4, 50)) { + SERIAL_ECHOLNPGM("?Sample size not plausible (4-50)."); + return; + } + + const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; + + // Test at the current position by default, overridden by X and Y + const xy_pos_t test_position = { + parser.linearval('X', current_position.x + probe.offset_xy.x), // If no X use the probe's current X position + parser.linearval('Y', current_position.y + probe.offset_xy.y) // If no Y, ditto + }; + + if (!probe.can_reach(test_position)) { + ui.set_status(GET_TEXT_F(MSG_M48_OUT_OF_BOUNDS), 99); + SERIAL_ECHOLNPGM("? (X,Y) out of bounds."); + return; + } + + // Get the number of leg moves per test-point + bool seen_L = parser.seen('L'); + uint8_t n_legs = seen_L ? parser.value_byte() : 0; + if (n_legs > 15) { + SERIAL_ECHOLNPGM("?Legs of movement implausible (0-15)."); + return; + } + if (n_legs == 1) n_legs = 2; + + // Schizoid motion as an optional stress-test + const bool schizoid_flag = parser.boolval('S'); + if (schizoid_flag && !seen_L) n_legs = 7; + + if (verbose_level > 2) + SERIAL_ECHOLNPGM("Positioning the probe..."); + + // Always disable Bed Level correction before probing... + + #if HAS_LEVELING + const bool was_enabled = planner.leveling_active; + set_bed_leveling_enabled(false); + #endif + + TERN_(HAS_PTC, ptc.set_enabled(!parser.seen('C') || parser.value_bool())); + + // Work with reasonable feedrates + remember_feedrate_scaling_off(); + + // Working variables + float mean = 0.0, // The average of all points so far, used to calculate deviation + sigma = 0.0, // Standard deviation of all points so far + min = 99999.9, // Smallest value sampled so far + max = -99999.9, // Largest value sampled so far + sample_set[n_samples]; // Storage for sampled values + + auto dev_report = [](const bool verbose, const_float_t mean, const_float_t sigma, const_float_t min, const_float_t max, const bool final=false) { + if (verbose) { + SERIAL_ECHOPAIR_F("Mean: ", mean, 6); + if (!final) SERIAL_ECHOPAIR_F(" Sigma: ", sigma, 6); + SERIAL_ECHOPAIR_F(" Min: ", min, 3); + SERIAL_ECHOPAIR_F(" Max: ", max, 3); + SERIAL_ECHOPAIR_F(" Range: ", max-min, 3); + if (final) SERIAL_EOL(); + } + if (final) { + SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6); + SERIAL_EOL(); + } + }; + + // Move to the first point, deploy, and probe + const float t = probe.probe_at_point(test_position, raise_after, verbose_level); + bool probing_good = !isnan(t); + + if (probing_good) { + randomSeed(millis()); + + float sample_sum = 0.0; + + LOOP_L_N(n, n_samples) { + #if HAS_STATUS_MESSAGE + // Display M48 progress in the status bar + ui.status_printf(0, F(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples)); + #endif + + // When there are "legs" of movement move around the point before probing + if (n_legs) { + + // Pick a random direction, starting angle, and radius + const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise + float angle = random(0, 360); + const float radius = random( + #if ENABLED(DELTA) + int(0.1250000000 * (DELTA_PRINTABLE_RADIUS)), + int(0.3333333333 * (DELTA_PRINTABLE_RADIUS)) + #else + int(5), int(0.125 * _MIN(X_BED_SIZE, Y_BED_SIZE)) + #endif + ); + if (verbose_level > 3) { + SERIAL_ECHOPGM("Start radius:", radius, " angle:", angle, " dir:"); + if (dir > 0) SERIAL_CHAR('C'); + SERIAL_ECHOLNPGM("CW"); + } + + // Move from leg to leg in rapid succession + LOOP_L_N(l, n_legs - 1) { + + // Move some distance around the perimeter + float delta_angle; + if (schizoid_flag) { + // The points of a 5 point star are 72 degrees apart. + // Skip a point and go to the next one on the star. + delta_angle = dir * 2.0 * 72.0; + } + else { + // Just move further along the perimeter. + delta_angle = dir * (float)random(25, 45); + } + angle += delta_angle; + + // Trig functions work without clamping, but just to be safe... + while (angle > 360.0) angle -= 360.0; + while (angle < 0.0) angle += 360.0; + + // Choose the next position as an offset to chosen test position + const xy_pos_t noz_pos = test_position - probe.offset_xy; + xy_pos_t next_pos = { + noz_pos.x + float(cos(RADIANS(angle))) * radius, + noz_pos.y + float(sin(RADIANS(angle))) * radius + }; + + #if ENABLED(DELTA) + // If the probe can't reach the point on a round bed... + // Simply scale the numbers to bring them closer to origin. + while (!probe.can_reach(next_pos)) { + next_pos *= 0.8f; + if (verbose_level > 3) + SERIAL_ECHOLNPGM_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y); + } + #elif HAS_ENDSTOPS + // For a rectangular bed just keep the probe in bounds + LIMIT(next_pos.x, X_MIN_POS, X_MAX_POS); + LIMIT(next_pos.y, Y_MIN_POS, Y_MAX_POS); + #endif + + if (verbose_level > 3) + SERIAL_ECHOLNPGM_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y); + + do_blocking_move_to_xy(next_pos); + } // n_legs loop + } // n_legs + + // Probe a single point + const float pz = probe.probe_at_point(test_position, raise_after, 0); + + // Break the loop if the probe fails + probing_good = !isnan(pz); + if (!probing_good) break; + + // Store the new sample + sample_set[n] = pz; + + // Keep track of the largest and smallest samples + NOMORE(min, pz); + NOLESS(max, pz); + + // Get the mean value of all samples thus far + sample_sum += pz; + mean = sample_sum / (n + 1); + + // Calculate the standard deviation so far. + // The value after the last sample will be the final output. + float dev_sum = 0.0; + LOOP_LE_N(j, n) dev_sum += sq(sample_set[j] - mean); + sigma = SQRT(dev_sum / (n + 1)); + + if (verbose_level > 1) { + SERIAL_ECHO(n + 1); + SERIAL_ECHOPGM(" of ", n_samples); + SERIAL_ECHOPAIR_F(": z: ", pz, 3); + SERIAL_CHAR(' '); + dev_report(verbose_level > 2, mean, sigma, min, max); + SERIAL_EOL(); + } + + } // n_samples loop + } + + probe.stow(); + + if (probing_good) { + SERIAL_ECHOLNPGM("Finished!"); + dev_report(verbose_level > 0, mean, sigma, min, max, true); + + #if HAS_STATUS_MESSAGE + // Display M48 results in the status bar + char sigma_str[8]; + ui.status_printf(0, F(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str)); + #endif + } + + restore_feedrate_and_scaling(); + + // Re-enable bed level correction if it had been on + TERN_(HAS_LEVELING, set_bed_leveling_enabled(was_enabled)); + + // Re-enable probe temperature correction + TERN_(HAS_PTC, ptc.set_enabled(true)); + + report_current_position(); +} + +#endif // Z_MIN_PROBE_REPEATABILITY_TEST diff --git a/src/gcode/calibrate/M665.cpp b/src/gcode/calibrate/M665.cpp new file mode 100644 index 0000000..7dc657a --- /dev/null +++ b/src/gcode/calibrate/M665.cpp @@ -0,0 +1,188 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if IS_KINEMATIC + +#include "../gcode.h" +#include "../../module/motion.h" + +#if ENABLED(DELTA) + + #include "../../module/delta.h" + + /** + * M665: Set delta configurations + * + * H = delta height + * L = diagonal rod + * R = delta radius + * S = segments per second + * X = Alpha (Tower 1) angle trim + * Y = Beta (Tower 2) angle trim + * Z = Gamma (Tower 3) angle trim + * A = Alpha (Tower 1) diagonal rod trim + * B = Beta (Tower 2) diagonal rod trim + * C = Gamma (Tower 3) diagonal rod trim + */ + void GcodeSuite::M665() { + if (!parser.seen_any()) return M665_report(); + + if (parser.seenval('H')) delta_height = parser.value_linear_units(); + if (parser.seenval('L')) delta_diagonal_rod = parser.value_linear_units(); + if (parser.seenval('R')) delta_radius = parser.value_linear_units(); + if (parser.seenval('S')) segments_per_second = parser.value_float(); + if (parser.seenval('X')) delta_tower_angle_trim.a = parser.value_float(); + if (parser.seenval('Y')) delta_tower_angle_trim.b = parser.value_float(); + if (parser.seenval('Z')) delta_tower_angle_trim.c = parser.value_float(); + if (parser.seenval('A')) delta_diagonal_rod_trim.a = parser.value_float(); + if (parser.seenval('B')) delta_diagonal_rod_trim.b = parser.value_float(); + if (parser.seenval('C')) delta_diagonal_rod_trim.c = parser.value_float(); + recalc_delta_settings(); + } + + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_DELTA_SETTINGS)); + SERIAL_ECHOLNPGM_P( + PSTR(" M665 L"), LINEAR_UNIT(delta_diagonal_rod) + , PSTR(" R"), LINEAR_UNIT(delta_radius) + , PSTR(" H"), LINEAR_UNIT(delta_height) + , PSTR(" S"), segments_per_second + , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a) + , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b) + , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c) + , PSTR(" A"), LINEAR_UNIT(delta_diagonal_rod_trim.a) + , PSTR(" B"), LINEAR_UNIT(delta_diagonal_rod_trim.b) + , PSTR(" C"), LINEAR_UNIT(delta_diagonal_rod_trim.c) + ); + } + +#elif IS_SCARA + + #include "../../module/scara.h" + + /** + * M665: Set SCARA settings + * + * Parameters: + * + * S[segments] - Segments-per-second + * + * Without NO_WORKSPACE_OFFSETS: + * + * P[theta-psi-offset] - Theta-Psi offset, added to the shoulder (A/X) angle + * T[theta-offset] - Theta offset, added to the elbow (B/Y) angle + * Z[z-offset] - Z offset, added to Z + * + * A, P, and X are all aliases for the shoulder angle + * B, T, and Y are all aliases for the elbow angle + */ + void GcodeSuite::M665() { + if (!parser.seen_any()) return M665_report(); + + if (parser.seenval('S')) segments_per_second = parser.value_float(); + + #if HAS_SCARA_OFFSET + + if (parser.seenval('Z')) scara_home_offset.z = parser.value_linear_units(); + + const bool hasA = parser.seenval('A'), hasP = parser.seenval('P'), hasX = parser.seenval('X'); + const uint8_t sumAPX = hasA + hasP + hasX; + if (sumAPX) { + if (sumAPX == 1) + scara_home_offset.a = parser.value_float(); + else { + SERIAL_ERROR_MSG("Only one of A, P, or X is allowed."); + return; + } + } + + const bool hasB = parser.seenval('B'), hasT = parser.seenval('T'), hasY = parser.seenval('Y'); + const uint8_t sumBTY = hasB + hasT + hasY; + if (sumBTY) { + if (sumBTY == 1) + scara_home_offset.b = parser.value_float(); + else { + SERIAL_ERROR_MSG("Only one of B, T, or Y is allowed."); + return; + } + } + + #endif // HAS_SCARA_OFFSET + } + + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SCARA_SETTINGS " (" STR_S_SEG_PER_SEC TERN_(HAS_SCARA_OFFSET, " " STR_SCARA_P_T_Z) ")")); + SERIAL_ECHOLNPGM_P( + PSTR(" M665 S"), segments_per_second + #if HAS_SCARA_OFFSET + , SP_P_STR, scara_home_offset.a + , SP_T_STR, scara_home_offset.b + , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z) + #endif + ); + } + +#elif ENABLED(POLARGRAPH) + + #include "../../module/polargraph.h" + + /** + * M665: Set POLARGRAPH settings + * + * Parameters: + * + * S[segments] - Segments-per-second + * L[left] - Work area minimum X + * R[right] - Work area maximum X + * T[top] - Work area maximum Y + * B[bottom] - Work area minimum Y + * H[length] - Maximum belt length + */ + void GcodeSuite::M665() { + if (!parser.seen_any()) return M665_report(); + if (parser.seenval('S')) segments_per_second = parser.value_float(); + if (parser.seenval('L')) draw_area_min.x = parser.value_linear_units(); + if (parser.seenval('R')) draw_area_max.x = parser.value_linear_units(); + if (parser.seenval('T')) draw_area_max.y = parser.value_linear_units(); + if (parser.seenval('B')) draw_area_min.y = parser.value_linear_units(); + if (parser.seenval('H')) polargraph_max_belt_len = parser.value_linear_units(); + draw_area_size.x = draw_area_max.x - draw_area_min.x; + draw_area_size.y = draw_area_max.y - draw_area_min.y; + } + + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_POLARGRAPH_SETTINGS)); + SERIAL_ECHOLNPGM_P( + PSTR(" M665 S"), LINEAR_UNIT(segments_per_second), + PSTR(" L"), LINEAR_UNIT(draw_area_min.x), + PSTR(" R"), LINEAR_UNIT(draw_area_max.x), + SP_T_STR, LINEAR_UNIT(draw_area_max.y), + SP_B_STR, LINEAR_UNIT(draw_area_min.y), + PSTR(" H"), LINEAR_UNIT(polargraph_max_belt_len) + ); + } + +#endif + +#endif // IS_KINEMATIC diff --git a/src/gcode/calibrate/M666.cpp b/src/gcode/calibrate/M666.cpp new file mode 100644 index 0000000..90fad18 --- /dev/null +++ b/src/gcode/calibrate/M666.cpp @@ -0,0 +1,133 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(DELTA) || HAS_EXTRA_ENDSTOPS + +#include "../gcode.h" + +#if ENABLED(DELTA) + #include "../../module/delta.h" + #include "../../module/motion.h" +#else + #include "../../module/endstops.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +#if ENABLED(DELTA) + + /** + * M666: Set delta endstop adjustment + */ + void GcodeSuite::M666() { + DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING)); + bool is_err = false, is_set = false; + LOOP_NUM_AXES(i) { + if (parser.seenval(AXIS_CHAR(i))) { + is_set = true; + const float v = parser.value_linear_units(); + if (v > 0) + is_err = true; + else { + delta_endstop_adj[i] = v; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("delta_endstop_adj[", AS_CHAR(AXIS_CHAR(i)), "] = ", v); + } + } + } + if (is_err) SERIAL_ECHOLNPGM("?M666 offsets must be <= 0"); + if (!is_set) M666_report(); + } + + void GcodeSuite::M666_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ENDSTOP_ADJUSTMENT)); + SERIAL_ECHOLNPGM_P( + PSTR(" M666 X"), LINEAR_UNIT(delta_endstop_adj.a) + , SP_Y_STR, LINEAR_UNIT(delta_endstop_adj.b) + , SP_Z_STR, LINEAR_UNIT(delta_endstop_adj.c) + ); + } + +#else + + /** + * M666: Set Dual Endstops offsets for X, Y, and/or Z. + * With no parameters report current offsets. + * + * For Triple / Quad Z Endstops: + * Set Z2 Only: M666 S2 Z + * Set Z3 Only: M666 S3 Z + * Set Z4 Only: M666 S4 Z + * Set All: M666 Z + */ + void GcodeSuite::M666() { + if (!parser.seen_any()) return M666_report(); + + #if ENABLED(X_DUAL_ENDSTOPS) + if (parser.seenval('X')) endstops.x2_endstop_adj = parser.value_linear_units(); + #endif + #if ENABLED(Y_DUAL_ENDSTOPS) + if (parser.seenval('Y')) endstops.y2_endstop_adj = parser.value_linear_units(); + #endif + #if ENABLED(Z_MULTI_ENDSTOPS) + if (parser.seenval('Z')) { + const float z_adj = parser.value_linear_units(); + #if NUM_Z_STEPPERS == 2 + endstops.z2_endstop_adj = z_adj; + #else + const int ind = parser.intval('S'); + #define _SET_ZADJ(N) if (!ind || ind == N) endstops.z##N##_endstop_adj = z_adj; + REPEAT_S(2, INCREMENT(NUM_Z_STEPPERS), _SET_ZADJ) + #endif + } + #endif + } + + void GcodeSuite::M666_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ENDSTOP_ADJUSTMENT)); + SERIAL_ECHOPGM(" M666"); + #if ENABLED(X_DUAL_ENDSTOPS) + SERIAL_ECHOLNPGM_P(SP_X_STR, LINEAR_UNIT(endstops.x2_endstop_adj)); + #endif + #if ENABLED(Y_DUAL_ENDSTOPS) + SERIAL_ECHOLNPGM_P(SP_Y_STR, LINEAR_UNIT(endstops.y2_endstop_adj)); + #endif + #if ENABLED(Z_MULTI_ENDSTOPS) + #if NUM_Z_STEPPERS >= 3 + SERIAL_ECHOPGM(" S2 Z", LINEAR_UNIT(endstops.z3_endstop_adj)); + report_echo_start(forReplay); + SERIAL_ECHOPGM(" M666 S3 Z", LINEAR_UNIT(endstops.z3_endstop_adj)); + #if NUM_Z_STEPPERS >= 4 + report_echo_start(forReplay); + SERIAL_ECHOPGM(" M666 S4 Z", LINEAR_UNIT(endstops.z4_endstop_adj)); + #endif + #else + SERIAL_ECHOLNPGM_P(SP_Z_STR, LINEAR_UNIT(endstops.z2_endstop_adj)); + #endif + #endif + } + +#endif // HAS_EXTRA_ENDSTOPS + +#endif // DELTA || HAS_EXTRA_ENDSTOPS diff --git a/src/gcode/calibrate/M852.cpp b/src/gcode/calibrate/M852.cpp new file mode 100644 index 0000000..6c661dc --- /dev/null +++ b/src/gcode/calibrate/M852.cpp @@ -0,0 +1,106 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SKEW_CORRECTION_GCODE) + +#include "../gcode.h" +#include "../../module/planner.h" + +/** + * M852: Get or set the machine skew factors. Reports current values with no arguments. + * + * S[xy_factor] - Alias for 'I' + * I[xy_factor] - New XY skew factor + * J[xz_factor] - New XZ skew factor + * K[yz_factor] - New YZ skew factor + */ +void GcodeSuite::M852() { + if (!parser.seen("SIJK")) return M852_report(); + + uint8_t badval = 0, setval = 0; + + if (parser.seenval('I') || parser.seenval('S')) { + const float value = parser.value_linear_units(); + if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { + if (planner.skew_factor.xy != value) { + planner.skew_factor.xy = value; + ++setval; + } + } + else + ++badval; + } + + #if ENABLED(SKEW_CORRECTION_FOR_Z) + + if (parser.seenval('J')) { + const float value = parser.value_linear_units(); + if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { + if (planner.skew_factor.xz != value) { + planner.skew_factor.xz = value; + ++setval; + } + } + else + ++badval; + } + + if (parser.seenval('K')) { + const float value = parser.value_linear_units(); + if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { + if (planner.skew_factor.yz != value) { + planner.skew_factor.yz = value; + ++setval; + } + } + else + ++badval; + } + + #endif + + if (badval) + SERIAL_ECHOLNPGM(STR_SKEW_MIN " " STRINGIFY(SKEW_FACTOR_MIN) " " STR_SKEW_MAX " " STRINGIFY(SKEW_FACTOR_MAX)); + + // When skew is changed the current position changes + if (setval) { + set_current_from_steppers_for_axis(ALL_AXES_ENUM); + sync_plan_position(); + report_current_position(); + } +} + +void GcodeSuite::M852_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SKEW_FACTOR)); + SERIAL_ECHOPAIR_F(" M852 I", planner.skew_factor.xy, 6); + #if ENABLED(SKEW_CORRECTION_FOR_Z) + SERIAL_ECHOPAIR_F(" J", planner.skew_factor.xz, 6); + SERIAL_ECHOPAIR_F(" K", planner.skew_factor.yz, 6); + SERIAL_ECHOLNPGM(" ; XY, XZ, YZ"); + #else + SERIAL_ECHOLNPGM(" ; XY"); + #endif +} + +#endif // SKEW_CORRECTION_GCODE diff --git a/src/gcode/config/M200-M205.cpp b/src/gcode/config/M200-M205.cpp new file mode 100644 index 0000000..4190d40 --- /dev/null +++ b/src/gcode/config/M200-M205.cpp @@ -0,0 +1,330 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../MarlinCore.h" +#include "../../module/planner.h" + +#if DISABLED(NO_VOLUMETRICS) + + /** + * M200: Set filament diameter and set E axis units to cubic units + * + * T - Optional extruder number. Current extruder if omitted. + * D - Set filament diameter and enable. D0 disables volumetric. + * S - Turn volumetric ON or OFF. + * + * With VOLUMETRIC_EXTRUDER_LIMIT: + * + * L - Volumetric extruder limit (in mm^3/sec). L0 disables the limit. + */ + void GcodeSuite::M200() { + if (!parser.seen("DST" TERN_(VOLUMETRIC_EXTRUDER_LIMIT, "L"))) + return M200_report(); + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + bool vol_enable = parser.volumetric_enabled, + can_enable = true; + + if (parser.seenval('D')) { + const float dval = parser.value_linear_units(); + if (dval) { // Set filament size for volumetric calculation + planner.set_filament_size(target_extruder, dval); + vol_enable = true; // Dn = enable for compatibility + } + else + can_enable = false; // D0 = disable for compatibility + } + + // Enable or disable with S1 / S0 + parser.volumetric_enabled = can_enable && parser.boolval('S', vol_enable); + + #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) + if (parser.seenval('L')) { + // Set volumetric limit (in mm^3/sec) + const float lval = parser.value_float(); + if (WITHIN(lval, 0, 20)) + planner.set_volumetric_extruder_limit(target_extruder, lval); + else + SERIAL_ECHOLNPGM("?L value out of range (0-20)."); + } + #endif + + planner.calculate_volumetric_multipliers(); + } + + void GcodeSuite::M200_report(const bool forReplay/*=true*/) { + if (!forReplay) { + report_heading(forReplay, F(STR_FILAMENT_SETTINGS), false); + if (!parser.volumetric_enabled) SERIAL_ECHOPGM(" (Disabled):"); + SERIAL_EOL(); + report_echo_start(forReplay); + } + + #if EXTRUDERS == 1 + { + SERIAL_ECHOLNPGM( + " M200 S", parser.volumetric_enabled, " D", LINEAR_UNIT(planner.filament_size[0]) + #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) + , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[0]) + #endif + ); + } + #else + SERIAL_ECHOLNPGM(" M200 S", parser.volumetric_enabled); + EXTRUDER_LOOP() { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM( + " M200 T", e, " D", LINEAR_UNIT(planner.filament_size[e]) + #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) + , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[e]) + #endif + ); + } + #endif + } + +#endif // !NO_VOLUMETRICS + +/** + * M201: Set max acceleration in units/s^2 for print moves. + * + * X : Max Acceleration for X + * Y : Max Acceleration for Y + * Z : Max Acceleration for Z + * ... : etc + * E : Max Acceleration for Extruder + * T : Extruder index to set + * + * With XY_FREQUENCY_LIMIT: + * F : Frequency limit for XY...IJKUVW + * S : Speed factor percentage. + */ +void GcodeSuite::M201() { + if (!parser.seen("T" STR_AXES_LOGICAL TERN_(XY_FREQUENCY_LIMIT, "FS"))) + return M201_report(); + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + #ifdef XY_FREQUENCY_LIMIT + if (parser.seenval('F')) planner.set_frequency_limit(parser.value_byte()); + if (parser.seenval('S')) planner.xy_freq_min_speed_factor = constrain(parser.value_float(), 1, 100) / 100; + #endif + + LOOP_LOGICAL_AXES(i) { + if (parser.seenval(AXIS_CHAR(i))) { + const AxisEnum a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? E_AXIS_N(target_extruder) : (AxisEnum)i), (AxisEnum)i); + planner.set_max_acceleration(a, parser.value_axis_units(a)); + } + } +} + +void GcodeSuite::M201_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_MAX_ACCELERATION)); + SERIAL_ECHOLNPGM_P( + LIST_N(DOUBLE(NUM_AXES), + PSTR(" M201 X"), LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[K_AXIS]) + ) + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS]) + #endif + ); + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M201 T"), i + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS_N(i)]) + ); + } + #endif +} + +/** + * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec + * + * With multiple extruders use T to specify which one. + */ +void GcodeSuite::M203() { + if (!parser.seen("T" STR_AXES_LOGICAL)) + return M203_report(); + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + LOOP_LOGICAL_AXES(i) + if (parser.seenval(AXIS_CHAR(i))) { + const AxisEnum a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? E_AXIS_N(target_extruder) : (AxisEnum)i), (AxisEnum)i); + planner.set_max_feedrate(a, parser.value_axis_units(a)); + } +} + +void GcodeSuite::M203_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_MAX_FEEDRATES)); + SERIAL_ECHOLNPGM_P( + LIST_N(DOUBLE(NUM_AXES), + PSTR(" M203 X"), LINEAR_UNIT(planner.settings.max_feedrate_mm_s[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[K_AXIS]) + ) + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS]) + #endif + ); + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + if (!forReplay) SERIAL_ECHO_START(); + SERIAL_ECHOLNPGM_P( + PSTR(" M203 T"), i + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS_N(i)]) + ); + } + #endif +} + +/** + * M204: Set Accelerations in units/sec^2 (M204 P1200 R3000 T3000) + * + * P = Printing moves + * R = Retract only (no X, Y, Z) moves + * T = Travel (non printing) moves + */ +void GcodeSuite::M204() { + if (!parser.seen("PRST")) + return M204_report(); + else { + //planner.synchronize(); + // 'S' for legacy compatibility. Should NOT BE USED for new development + if (parser.seenval('S')) planner.settings.travel_acceleration = planner.settings.acceleration = parser.value_linear_units(); + if (parser.seenval('P')) planner.settings.acceleration = parser.value_linear_units(); + if (parser.seenval('R')) planner.settings.retract_acceleration = parser.value_linear_units(); + if (parser.seenval('T')) planner.settings.travel_acceleration = parser.value_linear_units(); + } +} + +void GcodeSuite::M204_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ACCELERATION_P_R_T)); + SERIAL_ECHOLNPGM_P( + PSTR(" M204 P"), LINEAR_UNIT(planner.settings.acceleration) + , PSTR(" R"), LINEAR_UNIT(planner.settings.retract_acceleration) + , SP_T_STR, LINEAR_UNIT(planner.settings.travel_acceleration) + ); +} + +/** + * M205: Set Advanced Settings + * + * B = Min Segment Time (µs) + * S = Min Feed Rate (units/s) + * T = Min Travel Feed Rate (units/s) + * X = Max X Jerk (units/sec^2) + * Y = Max Y Jerk (units/sec^2) + * Z = Max Z Jerk (units/sec^2) + * E = Max E Jerk (units/sec^2) + * J = Junction Deviation (mm) (If not using CLASSIC_JERK) + */ +void GcodeSuite::M205() { + if (!parser.seen("BST" TERN_(HAS_JUNCTION_DEVIATION, "J") TERN_(HAS_CLASSIC_JERK, "XYZE"))) + return M205_report(); + + //planner.synchronize(); + if (parser.seenval('B')) planner.settings.min_segment_time_us = parser.value_ulong(); + if (parser.seenval('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units(); + if (parser.seenval('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units(); + #if HAS_JUNCTION_DEVIATION + #if HAS_CLASSIC_JERK && AXIS_COLLISION('J') + #error "Can't set_max_jerk for 'J' axis because 'J' is used for Junction Deviation." + #endif + if (parser.seenval('J')) { + const float junc_dev = parser.value_linear_units(); + if (WITHIN(junc_dev, 0.01f, 0.3f)) { + planner.junction_deviation_mm = junc_dev; + TERN_(LIN_ADVANCE, planner.recalculate_max_e_jerk()); + } + else + SERIAL_ERROR_MSG("?J out of range (0.01 to 0.3)"); + } + #endif + #if HAS_CLASSIC_JERK + bool seenZ = false; + LOGICAL_AXIS_CODE( + if (parser.seenval('E')) planner.set_max_jerk(E_AXIS, parser.value_linear_units()), + if (parser.seenval('X')) planner.set_max_jerk(X_AXIS, parser.value_linear_units()), + if (parser.seenval('Y')) planner.set_max_jerk(Y_AXIS, parser.value_linear_units()), + if ((seenZ = parser.seenval('Z'))) planner.set_max_jerk(Z_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS4_NAME)) planner.set_max_jerk(I_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS5_NAME)) planner.set_max_jerk(J_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS6_NAME)) planner.set_max_jerk(K_AXIS, parser.value_linear_units()) + ); + #if HAS_MESH && DISABLED(LIMITED_JERK_EDITING) + if (seenZ && planner.max_jerk.z <= 0.1f) + SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses."); + #endif + #endif // HAS_CLASSIC_JERK +} + +void GcodeSuite::M205_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F( + "Advanced (B S T" + TERN_(HAS_JUNCTION_DEVIATION, " J") + #if HAS_CLASSIC_JERK + NUM_AXIS_GANG( + " X", " Y", " Z", + " " STR_I "", " " STR_J "", " " STR_K "" + ) + #endif + TERN_(HAS_CLASSIC_E_JERK, " E") + ")" + )); + SERIAL_ECHOLNPGM_P( + PSTR(" M205 B"), LINEAR_UNIT(planner.settings.min_segment_time_us) + , PSTR(" S"), LINEAR_UNIT(planner.settings.min_feedrate_mm_s) + , SP_T_STR, LINEAR_UNIT(planner.settings.min_travel_feedrate_mm_s) + #if HAS_JUNCTION_DEVIATION + , PSTR(" J"), LINEAR_UNIT(planner.junction_deviation_mm) + #endif + #if HAS_CLASSIC_JERK + , LIST_N(DOUBLE(NUM_AXES), + SP_X_STR, LINEAR_UNIT(planner.max_jerk.x), + SP_Y_STR, LINEAR_UNIT(planner.max_jerk.y), + SP_Z_STR, LINEAR_UNIT(planner.max_jerk.z), + SP_I_STR, LINEAR_UNIT(planner.max_jerk.i), + SP_J_STR, LINEAR_UNIT(planner.max_jerk.j), + SP_K_STR, LINEAR_UNIT(planner.max_jerk.k) + ) + #if HAS_CLASSIC_E_JERK + , SP_E_STR, LINEAR_UNIT(planner.max_jerk.e) + #endif + #endif + ); +} diff --git a/src/gcode/config/M217.cpp b/src/gcode/config/M217.cpp new file mode 100644 index 0000000..1c80858 --- /dev/null +++ b/src/gcode/config/M217.cpp @@ -0,0 +1,197 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_MULTI_EXTRUDER + +#include "../gcode.h" +#include "../../module/tool_change.h" + +#if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) + #include "../../module/motion.h" +#endif + +#include "../../MarlinCore.h" // for SP_X_STR, etc. + +/** + * M217 - Set toolchange parameters + * + * // Tool change command + * Q Prime active tool and exit + * + * // Tool change settings + * S[linear] Swap length + * B[linear] Extra Swap resume length + * E[linear] Extra Prime length (as used by M217 Q) + * P[linear/min] Prime speed + * R[linear/min] Retract speed + * U[linear/min] UnRetract speed + * V[linear] 0/1 Enable auto prime first extruder used + * W[linear] 0/1 Enable park & Z Raise + * X[linear] Park X (Requires TOOLCHANGE_PARK) + * Y[linear] Park Y (Requires TOOLCHANGE_PARK) + * I[linear] Park I (Requires TOOLCHANGE_PARK and NUM_AXES >= 4) + * J[linear] Park J (Requires TOOLCHANGE_PARK and NUM_AXES >= 5) + * K[linear] Park K (Requires TOOLCHANGE_PARK and NUM_AXES >= 6) + * Z[linear] Z Raise + * F[speed] Fan Speed 0-255 + * D[seconds] Fan time + * + * Tool migration settings + * A[0|1] Enable auto-migration on runout + * L[index] Last extruder to use for auto-migration + * + * Tool migration command + * T[index] Migrate to next extruder or the given extruder + */ +void GcodeSuite::M217() { + + #if ENABLED(TOOLCHANGE_FILAMENT_SWAP) + + static constexpr float max_extrude = TERN(PREVENT_LENGTHY_EXTRUDE, EXTRUDE_MAXLENGTH, 500); + + if (parser.seen('Q')) { tool_change_prime(); return; } + + if (parser.seenval('S')) { const float v = parser.value_linear_units(); toolchange_settings.swap_length = constrain(v, 0, max_extrude); } + if (parser.seenval('B')) { const float v = parser.value_linear_units(); toolchange_settings.extra_resume = constrain(v, -10, 10); } + if (parser.seenval('E')) { const float v = parser.value_linear_units(); toolchange_settings.extra_prime = constrain(v, 0, max_extrude); } + if (parser.seenval('P')) { const int16_t v = parser.value_linear_units(); toolchange_settings.prime_speed = constrain(v, 10, 5400); } + if (parser.seenval('R')) { const int16_t v = parser.value_linear_units(); toolchange_settings.retract_speed = constrain(v, 10, 5400); } + if (parser.seenval('U')) { const int16_t v = parser.value_linear_units(); toolchange_settings.unretract_speed = constrain(v, 10, 5400); } + #if TOOLCHANGE_FS_FAN >= 0 && HAS_FAN + if (parser.seenval('F')) { const uint16_t v = parser.value_ushort(); toolchange_settings.fan_speed = constrain(v, 0, 255); } + if (parser.seenval('D')) { const uint16_t v = parser.value_ushort(); toolchange_settings.fan_time = constrain(v, 1, 30); } + #endif + #endif + + #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED) + if (parser.seenval('V')) { enable_first_prime = parser.value_linear_units(); } + #endif + + #if ENABLED(TOOLCHANGE_PARK) + if (parser.seenval('W')) { toolchange_settings.enable_park = parser.value_linear_units(); } + if (parser.seenval('X')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.x = constrain(v, X_MIN_POS, X_MAX_POS); } + #if HAS_Y_AXIS + if (parser.seenval('Y')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.y = constrain(v, Y_MIN_POS, Y_MAX_POS); } + #endif + #if HAS_I_AXIS + if (parser.seenval('I')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.i = constrain(v, I_MIN_POS, I_MAX_POS); } + #endif + #if HAS_J_AXIS + if (parser.seenval('J')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.j = constrain(v, J_MIN_POS, J_MAX_POS); } + #endif + #if HAS_K_AXIS + if (parser.seenval('K')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.k = constrain(v, K_MIN_POS, K_MAX_POS); } + #endif + #endif + + #if HAS_Z_AXIS + if (parser.seenval('Z')) { toolchange_settings.z_raise = parser.value_linear_units(); } + #endif + + #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) + migration.target = 0; // 0 = disabled + + if (parser.seenval('L')) { // Last + const int16_t lval = parser.value_int(); + if (WITHIN(lval, 0, EXTRUDERS - 1)) { + migration.last = lval; + migration.automode = (active_extruder < migration.last); + } + } + + if (parser.seen('A')) // Auto on/off + migration.automode = parser.value_bool(); + + if (parser.seen('T')) { // Migrate now + if (parser.has_value()) { + const int16_t tval = parser.value_int(); + if (WITHIN(tval, 0, EXTRUDERS - 1) && tval != active_extruder) { + migration.target = tval + 1; + extruder_migration(); + migration.target = 0; // disable + return; + } + else + migration.target = 0; // disable + } + else { + extruder_migration(); + return; + } + } + + #endif + + M217_report(); +} + +void GcodeSuite::M217_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_TOOL_CHANGING)); + + SERIAL_ECHOPGM(" M217"); + + #if ENABLED(TOOLCHANGE_FILAMENT_SWAP) + SERIAL_ECHOPGM(" S", LINEAR_UNIT(toolchange_settings.swap_length)); + SERIAL_ECHOPGM_P(SP_B_STR, LINEAR_UNIT(toolchange_settings.extra_resume), + SP_E_STR, LINEAR_UNIT(toolchange_settings.extra_prime), + SP_P_STR, LINEAR_UNIT(toolchange_settings.prime_speed)); + SERIAL_ECHOPGM(" R", LINEAR_UNIT(toolchange_settings.retract_speed), + " U", LINEAR_UNIT(toolchange_settings.unretract_speed), + " F", toolchange_settings.fan_speed, + " D", toolchange_settings.fan_time); + + #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) + SERIAL_ECHOPGM(" A", migration.automode); + SERIAL_ECHOPGM(" L", LINEAR_UNIT(migration.last)); + #endif + + #if ENABLED(TOOLCHANGE_PARK) + { + SERIAL_ECHOPGM(" W", LINEAR_UNIT(toolchange_settings.enable_park)); + SERIAL_ECHOPGM_P( + SP_X_STR, LINEAR_UNIT(toolchange_settings.change_point.x) + #if HAS_Y_AXIS + , SP_Y_STR, LINEAR_UNIT(toolchange_settings.change_point.y) + #endif + #if SECONDARY_AXES >= 1 + , LIST_N(DOUBLE(SECONDARY_AXES), + SP_I_STR, I_AXIS_UNIT(toolchange_settings.change_point.i), + SP_J_STR, J_AXIS_UNIT(toolchange_settings.change_point.j), + SP_K_STR, K_AXIS_UNIT(toolchange_settings.change_point.k) + ) + #endif + ); + } + #endif + + #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED) + SERIAL_ECHOPGM(" V", LINEAR_UNIT(enable_first_prime)); + #endif + + #endif + + SERIAL_ECHOLNPGM_P(SP_Z_STR, LINEAR_UNIT(toolchange_settings.z_raise)); +} + +#endif // HAS_MULTI_EXTRUDER diff --git a/src/gcode/config/M218.cpp b/src/gcode/config/M218.cpp new file mode 100644 index 0000000..c39447a --- /dev/null +++ b/src/gcode/config/M218.cpp @@ -0,0 +1,72 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_HOTEND_OFFSET + +#include "../gcode.h" +#include "../../module/motion.h" + +#if ENABLED(DELTA) + #include "../../module/planner.h" +#endif + +/** + * M218 - set hotend offset (in linear units) + * + * T + * X + * Y + * Z + */ +void GcodeSuite::M218() { + + if (!parser.seen_any()) return M218_report(); + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + if (parser.seenval('X')) hotend_offset[target_extruder].x = parser.value_linear_units(); + if (parser.seenval('Y')) hotend_offset[target_extruder].y = parser.value_linear_units(); + if (parser.seenval('Z')) hotend_offset[target_extruder].z = parser.value_linear_units(); + + #if ENABLED(DELTA) + if (target_extruder == active_extruder) + do_blocking_move_to_xy(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]); + #endif +} + +void GcodeSuite::M218_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_HOTEND_OFFSETS)); + LOOP_S_L_N(e, 1, HOTENDS) { + report_echo_start(forReplay); + SERIAL_ECHOPGM_P( + PSTR(" M218 T"), e, + SP_X_STR, LINEAR_UNIT(hotend_offset[e].x), + SP_Y_STR, LINEAR_UNIT(hotend_offset[e].y) + ); + SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, LINEAR_UNIT(hotend_offset[e].z), 3); + } +} + +#endif // HAS_HOTEND_OFFSET diff --git a/src/gcode/config/M220.cpp b/src/gcode/config/M220.cpp new file mode 100644 index 0000000..c9070df --- /dev/null +++ b/src/gcode/config/M220.cpp @@ -0,0 +1,51 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/motion.h" + +/** + * M220: Set speed percentage factor, aka "Feed Rate" + * + * Parameters + * S : Set the feed rate percentage factor + * + * Report the current speed percentage factor if no parameter is specified + * + * For MMU2 and MMU2S devices... + * B : Flag to back up the current factor + * R : Flag to restore the last-saved factor + */ +void GcodeSuite::M220() { + + static int16_t backup_feedrate_percentage = 100; + if (parser.seen('B')) backup_feedrate_percentage = feedrate_percentage; + if (parser.seen('R')) feedrate_percentage = backup_feedrate_percentage; + + if (parser.seenval('S')) feedrate_percentage = parser.value_int(); + + if (!parser.seen_any()) { + SERIAL_ECHOPGM("FR:", feedrate_percentage); + SERIAL_CHAR('%'); + SERIAL_EOL(); + } +} diff --git a/src/gcode/config/M221.cpp b/src/gcode/config/M221.cpp new file mode 100644 index 0000000..f653ade --- /dev/null +++ b/src/gcode/config/M221.cpp @@ -0,0 +1,47 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/planner.h" + +#if HAS_EXTRUDERS + +/** + * M221: Set extrusion percentage (M221 T0 S95) + */ +void GcodeSuite::M221() { + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + if (parser.seenval('S')) + planner.set_flow(target_extruder, parser.value_int()); + else { + SERIAL_ECHO_START(); + SERIAL_CHAR('E', '0' + target_extruder); + SERIAL_ECHOPGM(" Flow: ", planner.flow_percentage[target_extruder]); + SERIAL_CHAR('%'); + SERIAL_EOL(); + } +} + +#endif // EXTRUDERS diff --git a/src/gcode/config/M281.cpp b/src/gcode/config/M281.cpp new file mode 100644 index 0000000..e4ef3ab --- /dev/null +++ b/src/gcode/config/M281.cpp @@ -0,0 +1,78 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(EDITABLE_SERVO_ANGLES) + +#include "../gcode.h" +#include "../../module/servo.h" + +/** + * M281 - Edit / Report Servo Angles + * + * P - Servo to update + * L - Deploy Angle + * U - Stowed Angle + */ +void GcodeSuite::M281() { + if (!parser.seen_any()) return M281_report(); + + if (!parser.seenval('P')) return; + + const int servo_index = parser.value_int(); + if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) { + #if ENABLED(BLTOUCH) + if (servo_index == Z_PROBE_SERVO_NR) { + SERIAL_ERROR_MSG("BLTouch angles can't be changed."); + return; + } + #endif + if (parser.seenval('L')) servo_angles[servo_index][0] = parser.value_int(); + if (parser.seenval('U')) servo_angles[servo_index][1] = parser.value_int(); + } + else + SERIAL_ERROR_MSG("Servo ", servo_index, " out of range"); +} + +void GcodeSuite::M281_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SERVO_ANGLES)); + LOOP_L_N(i, NUM_SERVOS) { + switch (i) { + default: break; + #if ENABLED(SWITCHING_EXTRUDER) + case SWITCHING_EXTRUDER_SERVO_NR: + #if EXTRUDERS > 3 + case SWITCHING_EXTRUDER_E23_SERVO_NR: + #endif + #elif ENABLED(SWITCHING_NOZZLE) + case SWITCHING_NOZZLE_SERVO_NR: + #elif ENABLED(BLTOUCH) || (HAS_Z_SERVO_PROBE && defined(Z_SERVO_ANGLES)) + case Z_PROBE_SERVO_NR: + #endif + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M281 P", i, " L", servo_angles[i][0], " U", servo_angles[i][1]); + } + } +} + +#endif // EDITABLE_SERVO_ANGLES diff --git a/src/gcode/config/M301.cpp b/src/gcode/config/M301.cpp new file mode 100644 index 0000000..fc9f188 --- /dev/null +++ b/src/gcode/config/M301.cpp @@ -0,0 +1,109 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PIDTEMP) + +#include "../gcode.h" +#include "../../module/temperature.h" + +/** + * M301: Set PID parameters P I D (and optionally C, L) + * + * E[extruder] Default: 0 + * + * P[float] Kp term + * I[float] Ki term (unscaled) + * D[float] Kd term (unscaled) + * + * With PID_EXTRUSION_SCALING: + * + * C[float] Kc term + * L[int] LPQ length + * + * With PID_FAN_SCALING: + * + * F[float] Kf term + */ +void GcodeSuite::M301() { + // multi-extruder PID patch: M301 updates or prints a single extruder's PID values + // default behavior (omitting E parameter) is to update for extruder 0 only + int8_t e = E_TERN0(parser.byteval('E', -1)); // extruder being updated + + if (!parser.seen("PID" TERN_(PID_EXTRUSION_SCALING, "CL") TERN_(PID_FAN_SCALING, "F"))) + return M301_report(true E_OPTARG(e)); + + if (e == -1) e = 0; + + if (e < HOTENDS) { // catch bad input value + + if (parser.seenval('P')) PID_PARAM(Kp, e) = parser.value_float(); + if (parser.seenval('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float()); + if (parser.seenval('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float()); + + #if ENABLED(PID_EXTRUSION_SCALING) + if (parser.seenval('C')) PID_PARAM(Kc, e) = parser.value_float(); + if (parser.seenval('L')) thermalManager.lpq_len = parser.value_int(); + NOMORE(thermalManager.lpq_len, LPQ_MAX_LEN); + NOLESS(thermalManager.lpq_len, 0); + #endif + + #if ENABLED(PID_FAN_SCALING) + if (parser.seenval('F')) PID_PARAM(Kf, e) = parser.value_float(); + #endif + + thermalManager.updatePID(); + } + else + SERIAL_ERROR_MSG(STR_INVALID_EXTRUDER); +} + +void GcodeSuite::M301_report(const bool forReplay/*=true*/ E_OPTARG(const int8_t eindex/*=-1*/)) { + report_heading(forReplay, F(STR_HOTEND_PID)); + IF_DISABLED(HAS_MULTI_EXTRUDER, constexpr int8_t eindex = -1); + HOTEND_LOOP() { + if (e == eindex || eindex == -1) { + report_echo_start(forReplay); + SERIAL_ECHOPGM_P( + #if ENABLED(PID_PARAMS_PER_HOTEND) + PSTR(" M301 E"), e, SP_P_STR + #else + PSTR(" M301 P") + #endif + , PID_PARAM(Kp, e) + , PSTR(" I"), unscalePID_i(PID_PARAM(Ki, e)) + , PSTR(" D"), unscalePID_d(PID_PARAM(Kd, e)) + ); + #if ENABLED(PID_EXTRUSION_SCALING) + SERIAL_ECHOPGM_P(SP_C_STR, PID_PARAM(Kc, e)); + if (e == 0) SERIAL_ECHOPGM(" L", thermalManager.lpq_len); + #endif + #if ENABLED(PID_FAN_SCALING) + SERIAL_ECHOPGM(" F", PID_PARAM(Kf, e)); + #endif + SERIAL_EOL(); + } + } +} + +#endif // PIDTEMP diff --git a/src/gcode/config/M302.cpp b/src/gcode/config/M302.cpp new file mode 100644 index 0000000..9f4d569 --- /dev/null +++ b/src/gcode/config/M302.cpp @@ -0,0 +1,68 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PREVENT_COLD_EXTRUSION) + +#include "../gcode.h" +#include "../../module/temperature.h" + +#if ENABLED(DWIN_LCD_PROUI) + #include "../../lcd/e3v2/proui/dwin_defines.h" +#endif + +/** + * M302: Allow cold extrudes, or set the minimum extrude temperature + * + * S sets the minimum extrude temperature + * P enables (1) or disables (0) cold extrusion + * + * Examples: + * + * M302 ; report current cold extrusion state + * M302 P0 ; enable cold extrusion checking + * M302 P1 ; disable cold extrusion checking + * M302 S0 ; always allow extrusion (disables checking) + * M302 S170 ; only allow extrusion above 170 + * M302 S170 P1 ; set min extrude temp to 170 but leave disabled + */ +void GcodeSuite::M302() { + const bool seen_S = parser.seen('S'); + if (seen_S) { + thermalManager.extrude_min_temp = parser.value_celsius(); + thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0); + TERN_(DWIN_LCD_PROUI, HMI_data.ExtMinT = thermalManager.extrude_min_temp); + } + + if (parser.seen('P')) + thermalManager.allow_cold_extrude = (thermalManager.extrude_min_temp == 0) || parser.value_bool(); + else if (!seen_S) { + // Report current state + SERIAL_ECHO_START(); + SERIAL_ECHOPGM("Cold extrudes are "); + SERIAL_ECHOF(thermalManager.allow_cold_extrude ? F("en") : F("dis")); + SERIAL_ECHOLNPGM("abled (min temp ", thermalManager.extrude_min_temp, "C)"); + } +} + +#endif // PREVENT_COLD_EXTRUSION diff --git a/src/gcode/config/M304.cpp b/src/gcode/config/M304.cpp new file mode 100644 index 0000000..c970288 --- /dev/null +++ b/src/gcode/config/M304.cpp @@ -0,0 +1,53 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PIDTEMPBED) + +#include "../gcode.h" +#include "../../module/temperature.h" + +/** + * M304 - Set and/or Report the current Bed PID values + * + * P - Set the P value + * I - Set the I value + * D - Set the D value + */ +void GcodeSuite::M304() { + if (!parser.seen("PID")) return M304_report(); + if (parser.seenval('P')) thermalManager.temp_bed.pid.Kp = parser.value_float(); + if (parser.seenval('I')) thermalManager.temp_bed.pid.Ki = scalePID_i(parser.value_float()); + if (parser.seenval('D')) thermalManager.temp_bed.pid.Kd = scalePID_d(parser.value_float()); +} + +void GcodeSuite::M304_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_BED_PID)); + SERIAL_ECHOLNPGM( + " M304 P", thermalManager.temp_bed.pid.Kp + , " I", unscalePID_i(thermalManager.temp_bed.pid.Ki) + , " D", unscalePID_d(thermalManager.temp_bed.pid.Kd) + ); +} + +#endif // PIDTEMPBED diff --git a/src/gcode/config/M305.cpp b/src/gcode/config/M305.cpp new file mode 100644 index 0000000..e774692 --- /dev/null +++ b/src/gcode/config/M305.cpp @@ -0,0 +1,79 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_USER_THERMISTORS + +#include "../gcode.h" +#include "../../module/temperature.h" + +/** + * M305: Set (or report) custom thermistor parameters + * + * P[index] Thermistor table index + * R[ohms] Pullup resistor value + * T[ohms] Resistance at 25C + * B[beta] Thermistor "beta" value + * C[coeff] Steinhart-Hart Coefficient 'C' + * + * Format: M305 P[tbl_index] R[pullup_resistor_val] T[therm_25C_resistance] B[therm_beta] C[Steinhart_Hart_C_coeff] + * + * Examples: M305 P0 R4700 T100000 B3950 C0.0 + * M305 P0 R4700 + * M305 P0 T100000 + * M305 P0 B3950 + * M305 P0 C0.0 + */ +void GcodeSuite::M305() { + const int8_t t_index = parser.intval('P', -1); + const bool do_set = parser.seen("BCRT"); + + // A valid P index is required + if (t_index >= (USER_THERMISTORS) || (do_set && t_index < 0)) + SERIAL_ECHO_MSG("!Invalid index. (0 <= P <= ", USER_THERMISTORS - 1, ")"); + else if (do_set) { + if (parser.seenval('R')) // Pullup resistor value + if (!thermalManager.set_pull_up_res(t_index, parser.value_float())) + SERIAL_ECHO_MSG("!Invalid series resistance. (0 < R < 1000000)"); + + if (parser.seenval('T')) // Resistance at 25C + if (!thermalManager.set_res25(t_index, parser.value_float())) + SERIAL_ECHO_MSG("!Invalid 25C resistance. (0 < T < 10000000)"); + + if (parser.seenval('B')) // Beta value + if (!thermalManager.set_beta(t_index, parser.value_float())) + SERIAL_ECHO_MSG("!Invalid beta. (0 < B < 1000000)"); + + if (parser.seenval('C')) // Steinhart-Hart C coefficient + if (!thermalManager.set_sh_coeff(t_index, parser.value_float())) + SERIAL_ECHO_MSG("!Invalid Steinhart-Hart C coeff. (-0.01 < C < +0.01)"); + } // If not setting then report parameters + else if (t_index < 0) { // ...all user thermistors + LOOP_L_N(i, USER_THERMISTORS) + thermalManager.M305_report(i); + } + else // ...one user thermistor + thermalManager.M305_report(t_index); +} + +#endif // HAS_USER_THERMISTORS diff --git a/src/gcode/config/M309.cpp b/src/gcode/config/M309.cpp new file mode 100644 index 0000000..5770232 --- /dev/null +++ b/src/gcode/config/M309.cpp @@ -0,0 +1,53 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PIDTEMPCHAMBER) + +#include "../gcode.h" +#include "../../module/temperature.h" + +/** + * M309 - Set and/or Report the current Chamber PID values + * + * P - Set the P value + * I - Set the I value + * D - Set the D value + */ +void GcodeSuite::M309() { + if (!parser.seen("PID")) return M309_report(); + if (parser.seen('P')) thermalManager.temp_chamber.pid.Kp = parser.value_float(); + if (parser.seen('I')) thermalManager.temp_chamber.pid.Ki = scalePID_i(parser.value_float()); + if (parser.seen('D')) thermalManager.temp_chamber.pid.Kd = scalePID_d(parser.value_float()); +} + +void GcodeSuite::M309_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_CHAMBER_PID)); + SERIAL_ECHOLNPGM( + " M309 P", thermalManager.temp_chamber.pid.Kp + , " I", unscalePID_i(thermalManager.temp_chamber.pid.Ki) + , " D", unscalePID_d(thermalManager.temp_chamber.pid.Kd) + ); +} + +#endif // PIDTEMPCHAMBER diff --git a/src/gcode/config/M43.cpp b/src/gcode/config/M43.cpp new file mode 100644 index 0000000..688b94c --- /dev/null +++ b/src/gcode/config/M43.cpp @@ -0,0 +1,386 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PINS_DEBUGGING) + +#include "../gcode.h" +#include "../../MarlinCore.h" // for pin_is_protected +#include "../../pins/pinsDebug.h" +#include "../../module/endstops.h" + +#if HAS_Z_SERVO_PROBE + #include "../../module/probe.h" + #include "../../module/servo.h" +#endif + +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + +#if ENABLED(HOST_PROMPT_SUPPORT) + #include "../../feature/host_actions.h" +#endif + +#if ENABLED(EXTENSIBLE_UI) + #include "../../lcd/extui/ui_api.h" +#endif + +#if HAS_RESUME_CONTINUE + #include "../../lcd/marlinui.h" +#endif + +#ifndef GET_PIN_MAP_PIN_M43 + #define GET_PIN_MAP_PIN_M43(Q) GET_PIN_MAP_PIN(Q) +#endif + +inline void toggle_pins() { + const bool ignore_protection = parser.boolval('I'); + const int repeat = parser.intval('R', 1), + start = PARSED_PIN_INDEX('S', 0), + end = PARSED_PIN_INDEX('L', NUM_DIGITAL_PINS - 1), + wait = parser.intval('W', 500); + + LOOP_S_LE_N(i, start, end) { + pin_t pin = GET_PIN_MAP_PIN_M43(i); + if (!VALID_PIN(pin)) continue; + if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) { + report_pin_state_extended(pin, ignore_protection, true, F("Untouched ")); + SERIAL_EOL(); + } + else { + hal.watchdog_refresh(); + report_pin_state_extended(pin, ignore_protection, true, F("Pulsing ")); + #ifdef __STM32F1__ + const auto prior_mode = _GET_MODE(i); + #else + const bool prior_mode = GET_PINMODE(pin); + #endif + #if AVR_AT90USB1286_FAMILY // Teensy IDEs don't know about these pins so must use FASTIO + if (pin == TEENSY_E2) { + SET_OUTPUT(TEENSY_E2); + for (int16_t j = 0; j < repeat; j++) { + WRITE(TEENSY_E2, LOW); safe_delay(wait); + WRITE(TEENSY_E2, HIGH); safe_delay(wait); + WRITE(TEENSY_E2, LOW); safe_delay(wait); + } + } + else if (pin == TEENSY_E3) { + SET_OUTPUT(TEENSY_E3); + for (int16_t j = 0; j < repeat; j++) { + WRITE(TEENSY_E3, LOW); safe_delay(wait); + WRITE(TEENSY_E3, HIGH); safe_delay(wait); + WRITE(TEENSY_E3, LOW); safe_delay(wait); + } + } + else + #endif + { + pinMode(pin, OUTPUT); + for (int16_t j = 0; j < repeat; j++) { + hal.watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait); + hal.watchdog_refresh(); extDigitalWrite(pin, 1); safe_delay(wait); + hal.watchdog_refresh(); extDigitalWrite(pin, 0); safe_delay(wait); + hal.watchdog_refresh(); + } + } + #ifdef __STM32F1__ + _SET_MODE(i, prior_mode); + #else + pinMode(pin, prior_mode); + #endif + } + SERIAL_EOL(); + } + SERIAL_ECHOLNPGM(STR_DONE); + +} // toggle_pins + +inline void servo_probe_test() { + + #if !(NUM_SERVOS > 0 && HAS_SERVO_0) + + SERIAL_ERROR_MSG("SERVO not set up."); + + #elif !HAS_Z_SERVO_PROBE + + SERIAL_ERROR_MSG("Z_PROBE_SERVO_NR not set up."); + + #else // HAS_Z_SERVO_PROBE + + const uint8_t probe_index = parser.byteval('P', Z_PROBE_SERVO_NR); + + SERIAL_ECHOLNPGM("Servo probe test\n" + ". using index: ", probe_index, + ", deploy angle: ", servo_angles[probe_index][0], + ", stow angle: ", servo_angles[probe_index][1] + ); + + bool deploy_state = false, stow_state; + + #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) + + #define PROBE_TEST_PIN Z_MIN_PIN + constexpr bool probe_inverting = Z_MIN_ENDSTOP_INVERTING; + + SERIAL_ECHOLNPGM(". Probe Z_MIN_PIN: ", PROBE_TEST_PIN); + SERIAL_ECHOPGM(". Z_MIN_ENDSTOP_INVERTING: "); + + #else + + #define PROBE_TEST_PIN Z_MIN_PROBE_PIN + constexpr bool probe_inverting = Z_MIN_PROBE_ENDSTOP_INVERTING; + + SERIAL_ECHOLNPGM(". Probe Z_MIN_PROBE_PIN: ", PROBE_TEST_PIN); + SERIAL_ECHOPGM( ". Z_MIN_PROBE_ENDSTOP_INVERTING: "); + + #endif + + serialprint_truefalse(probe_inverting); + SERIAL_EOL(); + + SET_INPUT_PULLUP(PROBE_TEST_PIN); + + // First, check for a probe that recognizes an advanced BLTouch sequence. + // In addition to STOW and DEPLOY, it uses SW MODE (and RESET in the beginning) + // to see if this is one of the following: BLTOUCH Classic 1.2, 1.3, or + // BLTouch Smart 1.0, 2.0, 2.2, 3.0, 3.1. But only if the user has actually + // configured a BLTouch as being present. If the user has not configured this, + // the BLTouch will be detected in the last phase of these tests (see further on). + bool blt = false; + // This code will try to detect a BLTouch probe or clone + #if ENABLED(BLTOUCH) + SERIAL_ECHOLNPGM(". Check for BLTOUCH"); + bltouch._reset(); + bltouch._stow(); + if (probe_inverting == READ(PROBE_TEST_PIN)) { + bltouch._set_SW_mode(); + if (probe_inverting != READ(PROBE_TEST_PIN)) { + bltouch._deploy(); + if (probe_inverting == READ(PROBE_TEST_PIN)) { + bltouch._stow(); + SERIAL_ECHOLNPGM("= BLTouch Classic 1.2, 1.3, Smart 1.0, 2.0, 2.2, 3.0, 3.1 detected."); + // Check for a 3.1 by letting the user trigger it, later + blt = true; + } + } + } + #endif + + // The following code is common to all kinds of servo probes. + // Since it could be a real servo or a BLTouch (any kind) or a clone, + // use only "common" functions - i.e. SERVO_MOVE. No bltouch.xxxx stuff. + + // If it is already recognised as a being a BLTouch, no need for this test + if (!blt) { + // DEPLOY and STOW 4 times and see if the signal follows + // Then it is a mechanical switch + uint8_t i = 0; + SERIAL_ECHOLNPGM(". Deploy & stow 4 times"); + do { + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + safe_delay(500); + deploy_state = READ(PROBE_TEST_PIN); + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + safe_delay(500); + stow_state = READ(PROBE_TEST_PIN); + } while (++i < 4); + + if (probe_inverting != deploy_state) SERIAL_ECHOLNPGM("WARNING: INVERTING setting probably backwards."); + + if (deploy_state != stow_state) { + SERIAL_ECHOLNPGM("= Mechanical Switch detected"); + if (deploy_state) { + SERIAL_ECHOLNPGM(" DEPLOYED state: HIGH (logic 1)", + " STOWED (triggered) state: LOW (logic 0)"); + } + else { + SERIAL_ECHOLNPGM(" DEPLOYED state: LOW (logic 0)", + " STOWED (triggered) state: HIGH (logic 1)"); + } + #if ENABLED(BLTOUCH) + SERIAL_ECHOLNPGM("FAIL: BLTOUCH enabled - Set up this device as a Servo Probe with INVERTING set to 'true'."); + #endif + return; + } + } + + // Ask the user for a trigger event and measure the pulse width. + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + safe_delay(500); + SERIAL_ECHOLNPGM("** Please trigger probe within 30 sec **"); + uint16_t probe_counter = 0; + + // Wait 30 seconds for user to trigger probe + for (uint16_t j = 0; j < 500 * 30 && probe_counter == 0 ; j++) { + safe_delay(2); + + if (0 == j % (500 * 1)) gcode.reset_stepper_timeout(); // Keep steppers powered + + if (deploy_state != READ(PROBE_TEST_PIN)) { // probe triggered + for (probe_counter = 0; probe_counter < 15 && deploy_state != READ(PROBE_TEST_PIN); ++probe_counter) safe_delay(2); + + SERIAL_ECHOPGM(". Pulse width"); + if (probe_counter == 15) + SERIAL_ECHOLNPGM(": 30ms or more"); + else + SERIAL_ECHOLNPGM(" (+/- 4ms): ", probe_counter * 2); + + if (probe_counter >= 4) { + if (probe_counter == 15) { + if (blt) SERIAL_ECHOPGM("= BLTouch V3.1"); + else SERIAL_ECHOPGM("= Z Servo Probe"); + } + else SERIAL_ECHOPGM("= BLTouch pre V3.1 (or compatible)"); + SERIAL_ECHOLNPGM(" detected."); + } + else SERIAL_ECHOLNPGM("FAIL: Noise detected - please re-run test"); + + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + return; + } + } + + if (!probe_counter) SERIAL_ECHOLNPGM("FAIL: No trigger detected"); + + #endif // HAS_Z_SERVO_PROBE + +} // servo_probe_test + +/** + * M43: Pin debug - report pin state, watch pins, toggle pins and servo probe test/report + * + * M43 - report name and state of pin(s) + * P Pin to read or watch. If omitted, reads all pins. + * I Flag to ignore Marlin's pin protection. + * + * M43 W - Watch pins -reporting changes- until reset, click, or M108. + * P Pin to read or watch. If omitted, read/watch all pins. + * I Flag to ignore Marlin's pin protection. + * + * M43 E - Enable / disable background endstop monitoring + * - Machine continues to operate + * - Reports changes to endstops + * - Toggles LED_PIN when an endstop changes + * - Cannot reliably catch the 5mS pulse from BLTouch type probes + * + * M43 T - Toggle pin(s) and report which pin is being toggled + * S - Start Pin number. If not given, will default to 0 + * L - End Pin number. If not given, will default to last pin defined for this board + * I - Flag to ignore Marlin's pin protection. Use with caution!!!! + * R - Repeat pulses on each pin this number of times before continuing to next pin + * W - Wait time (in milliseconds) between pulses. If not given will default to 500 + * + * M43 S - Servo probe test + * P - Probe index (optional - defaults to 0 + */ +void GcodeSuite::M43() { + + // 'T' must be first. It uses 'S' and 'E' differently. + if (parser.seen('T')) return toggle_pins(); + + // 'E' Enable or disable endstop monitoring and return + if (parser.seen('E')) { + endstops.monitor_flag = parser.value_bool(); + SERIAL_ECHOPGM("endstop monitor "); + SERIAL_ECHOF(endstops.monitor_flag ? F("en") : F("dis")); + SERIAL_ECHOLNPGM("abled"); + return; + } + + // 'S' Run servo probe test and return + if (parser.seen('S')) return servo_probe_test(); + + // 'P' Get the range of pins to test or watch + uint8_t first_pin = PARSED_PIN_INDEX('P', 0), + last_pin = parser.seenval('P') ? first_pin : TERN(HAS_HIGH_ANALOG_PINS, NUM_DIGITAL_PINS, NUMBER_PINS_TOTAL) - 1; + + if (first_pin > last_pin) return; + + // 'I' to ignore protected pins + const bool ignore_protection = parser.boolval('I'); + + // 'W' Watch until click, M108, or reset + if (parser.boolval('W')) { + SERIAL_ECHOLNPGM("Watching pins"); + #ifdef ARDUINO_ARCH_SAM + NOLESS(first_pin, 2); // Don't hijack the UART pins + #endif + uint8_t pin_state[last_pin - first_pin + 1]; + LOOP_S_LE_N(i, first_pin, last_pin) { + pin_t pin = GET_PIN_MAP_PIN_M43(i); + if (!VALID_PIN(pin)) continue; + if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue; + pinMode(pin, INPUT_PULLUP); + delay(1); + /* + if (IS_ANALOG(pin)) + pin_state[pin - first_pin] = analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)); // int16_t pin_state[...] + else + //*/ + pin_state[i - first_pin] = extDigitalRead(pin); + } + + #if HAS_RESUME_CONTINUE + KEEPALIVE_STATE(PAUSED_FOR_USER); + wait_for_user = true; + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, F("M43 Wait Called"), FPSTR(CONTINUE_STR))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(F("M43 Wait Called"))); + #endif + + for (;;) { + LOOP_S_LE_N(i, first_pin, last_pin) { + pin_t pin = GET_PIN_MAP_PIN_M43(i); + if (!VALID_PIN(pin)) continue; + if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) continue; + const byte val = + /* + IS_ANALOG(pin) + ? analogRead(DIGITAL_PIN_TO_ANALOG_PIN(pin)) : // int16_t val + : + //*/ + extDigitalRead(pin); + if (val != pin_state[i - first_pin]) { + report_pin_state_extended(pin, ignore_protection, false); + pin_state[i - first_pin] = val; + } + } + + #if HAS_RESUME_CONTINUE + ui.update(); + if (!wait_for_user) break; + #endif + + safe_delay(200); + } + } + else { + // Report current state of selected pin(s) + LOOP_S_LE_N(i, first_pin, last_pin) { + pin_t pin = GET_PIN_MAP_PIN_M43(i); + if (VALID_PIN(pin)) report_pin_state_extended(pin, ignore_protection, true); + } + } +} + +#endif // PINS_DEBUGGING diff --git a/src/gcode/config/M540.cpp b/src/gcode/config/M540.cpp new file mode 100644 index 0000000..e751248 --- /dev/null +++ b/src/gcode/config/M540.cpp @@ -0,0 +1,40 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SD_ABORT_ON_ENDSTOP_HIT) + +#include "../gcode.h" +#include "../../module/planner.h" + +/** + * M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>) + */ +void GcodeSuite::M540() { + + if (parser.seen('S')) + planner.abort_on_endstop_hit = parser.value_bool(); + +} + +#endif // SD_ABORT_ON_ENDSTOP_HIT diff --git a/src/gcode/config/M575.cpp b/src/gcode/config/M575.cpp new file mode 100644 index 0000000..2c12428 --- /dev/null +++ b/src/gcode/config/M575.cpp @@ -0,0 +1,81 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(BAUD_RATE_GCODE) + +#include "../gcode.h" + +/** + * M575 - Change serial baud rate + * + * P - Serial port index. Omit for all. + * B - Baud rate (bits per second) + */ +void GcodeSuite::M575() { + int32_t baud = parser.ulongval('B'); + switch (baud) { + case 24: + case 96: + case 192: + case 384: + case 576: + case 1152: baud *= 100; break; + case 250: + case 500: baud *= 1000; break; + case 19: baud = 19200; break; + case 38: baud = 38400; break; + case 57: baud = 57600; break; + case 115: baud = 115200; break; + } + switch (baud) { + case 2400: case 9600: case 19200: case 38400: case 57600: + case 115200: case 250000: case 500000: case 1000000: { + const int8_t port = parser.intval('P', -99); + const bool set1 = (port == -99 || port == 0); + if (set1) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(0), " baud rate set to ", baud); + #if HAS_MULTI_SERIAL + const bool set2 = (port == -99 || port == 1); + if (set2) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(1), " baud rate set to ", baud); + #ifdef SERIAL_PORT_3 + const bool set3 = (port == -99 || port == 2); + if (set3) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(2), " baud rate set to ", baud); + #endif + #endif + + SERIAL_FLUSH(); + + if (set1) { MYSERIAL1.end(); MYSERIAL1.begin(baud); } + #if HAS_MULTI_SERIAL + if (set2) { MYSERIAL2.end(); MYSERIAL2.begin(baud); } + #ifdef SERIAL_PORT_3 + if (set3) { MYSERIAL3.end(); MYSERIAL3.begin(baud); } + #endif + #endif + + } break; + default: SERIAL_ECHO_MSG("?(B)aud rate implausible."); + } +} + +#endif // BAUD_RATE_GCODE diff --git a/src/gcode/config/M672.cpp b/src/gcode/config/M672.cpp new file mode 100644 index 0000000..257b494 --- /dev/null +++ b/src/gcode/config/M672.cpp @@ -0,0 +1,98 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD) + +#include "../gcode.h" +#include "../../HAL/shared/Delay.h" +#include "../parser.h" + +/** + * The Marlin format for the M672 command is different than shown in the Duet Smart Effector + * documentation https://duet3d.dozuki.com/Wiki/Smart_effector_and_carriage_adapters_for_delta_printer + * + * To set custom sensitivity: + * Duet: M672 S105:aaa:bbb + * Marlin: M672 Saaa + * + * (where aaa is the desired sensitivity and bbb is 255 - aaa). + * + * Revert sensitivity to factory settings: + * Duet: M672 S105:131:131 + * Marlin: M672 R + */ + +#define M672_PROGBYTE 105 // magic byte to start programming custom sensitivity +#define M672_ERASEBYTE 131 // magic byte to clear custom sensitivity + +// +// Smart Effector byte send protocol: +// +// 0 0 1 0 ... always 0010 +// b7 b6 b5 b4 ~b4 ... hi bits, NOT last bit +// b3 b2 b1 b0 ~b0 ... lo bits, NOT last bit +// +void M672_send(uint8_t b) { // bit rate requirement: 1kHz +/- 30% + LOOP_L_N(bits, 14) { + switch (bits) { + default: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80)); b <<= 1; break; } // send bit, shift next into place + case 7: + case 12: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80)); break; } // send bit. no shift + case 8: + case 13: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !(b & 0x80)); b <<= 1; break; } // send inverted previous bit + case 0: case 1: // 00 + case 3: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW); break; } // 0010 + case 2: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, HIGH); break; } // 001 + } + DELAY_US(1000); + } +} + +/** + * M672 - Set/reset Duet Smart Effector sensitivity + * + * One of these is required: + * S - 0-255 + * R - Flag to reset sensitivity to default + */ +void GcodeSuite::M672() { + if (parser.seen('R')) { + M672_send(M672_ERASEBYTE); + M672_send(M672_ERASEBYTE); + } + else if (parser.seenval('S')) { + const int8_t M672_sensitivity = parser.value_byte(); + M672_send(M672_PROGBYTE); + M672_send(M672_sensitivity); + M672_send(255 - M672_sensitivity); + } + else { + SERIAL_ECHO_MSG("!'S' or 'R' parameter required."); + return; + } + + OUT_WRITE(SMART_EFFECTOR_MOD_PIN, LOW); // Keep Smart Effector in NORMAL mode +} + +#endif // DUET_SMART_EFFECTOR && SMART_EFFECTOR_MOD_PIN diff --git a/src/gcode/config/M92.cpp b/src/gcode/config/M92.cpp new file mode 100644 index 0000000..b2ec3be --- /dev/null +++ b/src/gcode/config/M92.cpp @@ -0,0 +1,120 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/planner.h" + +/** + * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, [I, [J, [K]]] and E. + * (Follows the same syntax as G92) + * + * With multiple extruders use T to specify which one. + * + * If no argument is given print the current values. + * + * With MAGIC_NUMBERS_GCODE: + * + * Use 'H' and/or 'L' to get ideal layer-height information. + * H - Specify micro-steps to use. Best guess if not supplied. + * L - Desired layer height in current units. Nearest good heights are shown. + */ +void GcodeSuite::M92() { + + const int8_t target_extruder = get_target_extruder_from_command(); + if (target_extruder < 0) return; + + // No arguments? Show M92 report. + if (!parser.seen(STR_AXES_LOGICAL TERN_(MAGIC_NUMBERS_GCODE, "HL"))) + return M92_report(true, target_extruder); + + LOOP_LOGICAL_AXES(i) { + if (parser.seenval(AXIS_CHAR(i))) { + if (TERN1(HAS_EXTRUDERS, i != E_AXIS)) + planner.settings.axis_steps_per_mm[i] = parser.value_per_axis_units((AxisEnum)i); + else { + #if HAS_EXTRUDERS + const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder))); + if (value < 20) { + float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab. + #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK + planner.max_jerk.e *= factor; + #endif + planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor; + planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor; + } + planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value; + #endif + } + } + } + planner.refresh_positioning(); + + #if ENABLED(MAGIC_NUMBERS_GCODE) + #ifndef Z_MICROSTEPS + #define Z_MICROSTEPS 16 + #endif + const float wanted = parser.linearval('L'); + if (parser.seen('H') || wanted) { + const uint16_t argH = parser.ushortval('H'), + micro_steps = argH ?: Z_MICROSTEPS; + const float z_full_step_mm = micro_steps * planner.mm_per_step[Z_AXIS]; + SERIAL_ECHO_START(); + SERIAL_ECHOPGM("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm); + if (wanted) { + const float best = uint16_t(wanted / z_full_step_mm) * z_full_step_mm; + SERIAL_ECHOPGM(", best:[", best); + if (best != wanted) { SERIAL_CHAR(','); SERIAL_DECIMAL(best + z_full_step_mm); } + SERIAL_CHAR(']'); + } + SERIAL_ECHOLNPGM(" }"); + } + #endif +} + +void GcodeSuite::M92_report(const bool forReplay/*=true*/, const int8_t e/*=-1*/) { + report_heading_etc(forReplay, F(STR_STEPS_PER_UNIT)); + SERIAL_ECHOPGM_P(LIST_N(DOUBLE(NUM_AXES), + PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[K_AXIS])) + ); + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + SERIAL_ECHOPGM_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS])); + #endif + SERIAL_EOL(); + + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + if (e >= 0 && i != e) continue; + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M92 T"), i, + SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)]) + ); + } + #else + UNUSED(e); + #endif +} diff --git a/src/gcode/control/M10-M11.cpp b/src/gcode/control/M10-M11.cpp new file mode 100644 index 0000000..d5a69dc --- /dev/null +++ b/src/gcode/control/M10-M11.cpp @@ -0,0 +1,44 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(AIR_EVACUATION) + +#include "../gcode.h" +#include "../../feature/spindle_laser.h" + +/** + * M10: Vacuum or Blower On + */ +void GcodeSuite::M10() { + cutter.air_evac_enable(); // Turn on Vacuum or Blower motor +} + +/** + * M11: Vacuum or Blower OFF + */ +void GcodeSuite::M11() { + cutter.air_evac_disable(); // Turn off Vacuum or Blower motor +} + +#endif // AIR_EVACUATION diff --git a/src/gcode/control/M108_M112_M410.cpp b/src/gcode/control/M108_M112_M410.cpp new file mode 100644 index 0000000..39f9c04 --- /dev/null +++ b/src/gcode/control/M108_M112_M410.cpp @@ -0,0 +1,56 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if DISABLED(EMERGENCY_PARSER) + +#include "../gcode.h" +#include "../../MarlinCore.h" // for wait_for_heatup, kill, M112_KILL_STR +#include "../../module/motion.h" // for quickstop_stepper + +/** + * M108: Stop the waiting for heaters in M109, M190, M303. Does not affect the target temperature. + */ +void GcodeSuite::M108() { + TERN_(HAS_RESUME_CONTINUE, wait_for_user = false); + wait_for_heatup = false; +} + +/** + * M112: Full Shutdown + */ +void GcodeSuite::M112() { + kill(FPSTR(M112_KILL_STR), nullptr, true); +} + +/** + * M410: Quickstop - Abort all planned moves + * + * This will stop the carriages mid-move, so most likely they + * will be out of sync with the stepper position after this. + */ +void GcodeSuite::M410() { + quickstop_stepper(); +} + +#endif // !EMERGENCY_PARSER diff --git a/src/gcode/control/M111.cpp b/src/gcode/control/M111.cpp new file mode 100644 index 0000000..a92d334 --- /dev/null +++ b/src/gcode/control/M111.cpp @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" + +/** + * M111: Set the debug level + */ +void GcodeSuite::M111() { + if (parser.seenval('S')) marlin_debug_flags = parser.value_byte(); + + static PGMSTR(str_debug_1, STR_DEBUG_ECHO); + static PGMSTR(str_debug_2, STR_DEBUG_INFO); + static PGMSTR(str_debug_4, STR_DEBUG_ERRORS); + static PGMSTR(str_debug_8, STR_DEBUG_DRYRUN); + static PGMSTR(str_debug_16, STR_DEBUG_COMMUNICATION); + #if ENABLED(DEBUG_LEVELING_FEATURE) + static PGMSTR(str_debug_detail, STR_DEBUG_DETAIL); + #endif + + static PGM_P const debug_strings[] PROGMEM = { + str_debug_1, str_debug_2, str_debug_4, str_debug_8, str_debug_16, + TERN_(DEBUG_LEVELING_FEATURE, str_debug_detail) + }; + + SERIAL_ECHO_START(); + SERIAL_ECHOPGM(STR_DEBUG_PREFIX); + if (marlin_debug_flags) { + uint8_t comma = 0; + LOOP_L_N(i, COUNT(debug_strings)) { + if (TEST(marlin_debug_flags, i)) { + if (comma++) SERIAL_CHAR(','); + SERIAL_ECHOPGM_P((PGM_P)pgm_read_ptr(&debug_strings[i])); + } + } + } + else { + SERIAL_ECHOPGM(STR_DEBUG_OFF); + #if !defined(__AVR__) || !defined(USBCON) + #if ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS) + SERIAL_ECHOPGM("\nBuffer Overruns: ", MYSERIAL1.buffer_overruns()); + #endif + + #if ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS) + SERIAL_ECHOPGM("\nFraming Errors: ", MYSERIAL1.framing_errors()); + #endif + + #if ENABLED(SERIAL_STATS_DROPPED_RX) + SERIAL_ECHOPGM("\nDropped bytes: ", MYSERIAL1.dropped()); + #endif + + #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) + SERIAL_ECHOPGM("\nMax RX Queue Size: ", MYSERIAL1.rxMaxEnqueued()); + #endif + #endif // !__AVR__ || !USBCON + } + SERIAL_EOL(); +} diff --git a/src/gcode/control/M120_M121.cpp b/src/gcode/control/M120_M121.cpp new file mode 100644 index 0000000..570f2e9 --- /dev/null +++ b/src/gcode/control/M120_M121.cpp @@ -0,0 +1,34 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/endstops.h" + +/** + * M120: Enable endstops and set non-homing endstop state to "enabled" + */ +void GcodeSuite::M120() { endstops.enable_globally(true); } + +/** + * M121: Disable endstops and set non-homing endstop state to "disabled" + */ +void GcodeSuite::M121() { endstops.enable_globally(false); } diff --git a/src/gcode/control/M17_M18_M84.cpp b/src/gcode/control/M17_M18_M84.cpp new file mode 100644 index 0000000..ebe48d9 --- /dev/null +++ b/src/gcode/control/M17_M18_M84.cpp @@ -0,0 +1,246 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../MarlinCore.h" // for stepper_inactive_time, disable_e_steppers +#include "../../lcd/marlinui.h" +#include "../../module/motion.h" // for e_axis_mask +#include "../../module/planner.h" +#include "../../module/stepper.h" + +#if ENABLED(AUTO_BED_LEVELING_UBL) + #include "../../feature/bedlevel/bedlevel.h" +#endif + +#define DEBUG_OUT ENABLED(MARLIN_DEV_MODE) +#include "../../core/debug_out.h" +#include "../../libs/hex_print.h" + +inline stepper_flags_t selected_axis_bits() { + stepper_flags_t selected{0}; + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (E_TERN0(parser.has_value())) { + const uint8_t e = parser.value_int(); + if (e < EXTRUDERS) + selected.bits = _BV(INDEX_OF_AXIS(E_AXIS, e)); + } + else + selected.bits = e_axis_mask; + } + #endif + selected.bits |= NUM_AXIS_GANG( + (parser.seen_test('X') << X_AXIS), + | (parser.seen_test('Y') << Y_AXIS), + | (parser.seen_test('Z') << Z_AXIS), + | (parser.seen_test(AXIS4_NAME) << I_AXIS), + | (parser.seen_test(AXIS5_NAME) << J_AXIS), + | (parser.seen_test(AXIS6_NAME) << K_AXIS) + ); + return selected; +} + +// Enable specified axes and warn about other affected axes +void do_enable(const stepper_flags_t to_enable) { + const ena_mask_t was_enabled = stepper.axis_enabled.bits, + shall_enable = to_enable.bits & ~was_enabled; + + DEBUG_ECHOLNPGM("Now Enabled: ", hex_word(stepper.axis_enabled.bits), " Enabling: ", hex_word(to_enable.bits), " | ", shall_enable); + + if (!shall_enable) return; // All specified axes already enabled? + + ena_mask_t also_enabled = 0; // Track steppers enabled due to overlap + + // Enable all flagged axes + LOOP_NUM_AXES(a) { + if (TEST(shall_enable, a)) { + stepper.enable_axis(AxisEnum(a)); // Mark and enable the requested axis + DEBUG_ECHOLNPGM("Enabled ", AXIS_CHAR(a), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... Enabled: ", hex_word(stepper.axis_enabled.bits)); + also_enabled |= enable_overlap[a]; + } + } + #if HAS_EXTRUDERS + EXTRUDER_LOOP() { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(shall_enable, a)) { + stepper.ENABLE_EXTRUDER(e); + DEBUG_ECHOLNPGM("Enabled E", AS_DIGIT(e), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... ", hex_word(stepper.axis_enabled.bits)); + also_enabled |= enable_overlap[a]; + } + } + #endif + + if ((also_enabled &= ~(shall_enable | was_enabled))) { + SERIAL_CHAR('('); + LOOP_NUM_AXES(a) if (TEST(also_enabled, a)) SERIAL_CHAR(AXIS_CHAR(a), ' '); + #if HAS_EXTRUDERS + #define _EN_ALSO(N) if (TEST(also_enabled, INDEX_OF_AXIS(E_AXIS, N))) SERIAL_CHAR('E', '0' + N, ' '); + REPEAT(EXTRUDERS, _EN_ALSO) + #endif + SERIAL_ECHOLNPGM("also enabled)"); + } + + DEBUG_ECHOLNPGM("Enabled Now: ", hex_word(stepper.axis_enabled.bits)); +} + +/** + * M17: Enable stepper motor power for one or more axes. + * Print warnings for axes that share an ENABLE_PIN. + * + * Examples: + * + * M17 XZ ; Enable X and Z axes + * M17 E ; Enable all E steppers + * M17 E1 ; Enable just the E1 stepper + */ +void GcodeSuite::M17() { + if (parser.seen_axis()) { + if (any_enable_overlap()) + do_enable(selected_axis_bits()); + else { + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (parser.has_value()) { + const uint8_t e = parser.value_int(); + if (e < EXTRUDERS) stepper.ENABLE_EXTRUDER(e); + } + else + stepper.enable_e_steppers(); + } + #endif + LOOP_NUM_AXES(a) + if (parser.seen_test(AXIS_CHAR(a))) stepper.enable_axis((AxisEnum)a); + } + } + else { + LCD_MESSAGE(MSG_NO_MOVE); + stepper.enable_all_steppers(); + } +} + +void try_to_disable(const stepper_flags_t to_disable) { + ena_mask_t still_enabled = to_disable.bits & stepper.axis_enabled.bits; + + DEBUG_ECHOLNPGM("Enabled: ", hex_word(stepper.axis_enabled.bits), " To Disable: ", hex_word(to_disable.bits), " | ", hex_word(still_enabled)); + + if (!still_enabled) return; + + // Attempt to disable all flagged axes + LOOP_NUM_AXES(a) + if (TEST(to_disable.bits, a)) { + DEBUG_ECHOPGM("Try to disable ", AXIS_CHAR(a), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... "); + if (stepper.disable_axis(AxisEnum(a))) { // Mark the requested axis and request to disable + DEBUG_ECHOPGM("OK"); + still_enabled &= ~(_BV(a) | enable_overlap[a]); // If actually disabled, clear one or more tracked bits + } + else + DEBUG_ECHOPGM("OVERLAP"); + DEBUG_ECHOLNPGM(" ... still_enabled=", hex_word(still_enabled)); + } + #if HAS_EXTRUDERS + EXTRUDER_LOOP() { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(to_disable.bits, a)) { + DEBUG_ECHOPGM("Try to disable E", AS_DIGIT(e), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... "); + if (stepper.DISABLE_EXTRUDER(e)) { + DEBUG_ECHOPGM("OK"); + still_enabled &= ~(_BV(a) | enable_overlap[a]); + } + else + DEBUG_ECHOPGM("OVERLAP"); + DEBUG_ECHOLNPGM(" ... still_enabled=", hex_word(still_enabled)); + } + } + #endif + + auto overlap_warning = [](const ena_mask_t axis_bits) { + SERIAL_ECHOPGM(" not disabled. Shared with"); + LOOP_NUM_AXES(a) if (TEST(axis_bits, a)) SERIAL_ECHOPGM_P((PGM_P)pgm_read_ptr(&SP_AXIS_STR[a])); + #if HAS_EXTRUDERS + #define _EN_STILLON(N) if (TEST(axis_bits, INDEX_OF_AXIS(E_AXIS, N))) SERIAL_CHAR(' ', 'E', '0' + N); + REPEAT(EXTRUDERS, _EN_STILLON) + #endif + SERIAL_ECHOLNPGM("."); + }; + + // If any of the requested axes are still enabled, give a warning + LOOP_NUM_AXES(a) { + if (TEST(still_enabled, a)) { + SERIAL_CHAR(AXIS_CHAR(a)); + overlap_warning(stepper.axis_enabled.bits & enable_overlap[a]); + } + } + #if HAS_EXTRUDERS + EXTRUDER_LOOP() { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(still_enabled, a)) { + SERIAL_CHAR('E', '0' + e); + overlap_warning(stepper.axis_enabled.bits & enable_overlap[a]); + } + } + #endif + + DEBUG_ECHOLNPGM("Enabled Now: ", hex_word(stepper.axis_enabled.bits)); +} + +/** + * M18, M84: Disable stepper motor power for one or more axes. + * Print warnings for axes that share an ENABLE_PIN. + */ +void GcodeSuite::M18_M84() { + if (parser.seenval('S')) { + reset_stepper_timeout(); + #if HAS_DISABLE_INACTIVE_AXIS + const millis_t ms = parser.value_millis_from_seconds(); + #if LASER_SAFETY_TIMEOUT_MS > 0 + if (ms && ms <= LASER_SAFETY_TIMEOUT_MS) { + SERIAL_ECHO_MSG("M18 timeout must be > ", MS_TO_SEC(LASER_SAFETY_TIMEOUT_MS + 999), " s for laser safety."); + return; + } + #endif + stepper_inactive_time = ms; + #endif + } + else { + if (parser.seen_axis()) { + planner.synchronize(); + if (any_enable_overlap()) + try_to_disable(selected_axis_bits()); + else { + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (E_TERN0(parser.has_value())) + stepper.DISABLE_EXTRUDER(parser.value_int()); + else + stepper.disable_e_steppers(); + } + #endif + LOOP_NUM_AXES(a) + if (parser.seen_test(AXIS_CHAR(a))) stepper.disable_axis((AxisEnum)a); + } + } + else + planner.finish_and_disable(); + + TERN_(AUTO_BED_LEVELING_UBL, bedlevel.steppers_were_disabled()); + } +} diff --git a/src/gcode/control/M211.cpp b/src/gcode/control/M211.cpp new file mode 100644 index 0000000..95ae052 --- /dev/null +++ b/src/gcode/control/M211.cpp @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if HAS_SOFTWARE_ENDSTOPS + +#include "../gcode.h" +#include "../../module/motion.h" + +/** + * M211: Enable, Disable, and/or Report software endstops + * + * Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report + */ +void GcodeSuite::M211() { + if (parser.seen('S')) + soft_endstop._enabled = parser.value_bool(); + else + M211_report(); +} + +void GcodeSuite::M211_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SOFT_ENDSTOPS)); + SERIAL_ECHOPGM(" M211 S", AS_DIGIT(soft_endstop._enabled), " ; "); + serialprintln_onoff(soft_endstop._enabled); + + report_echo_start(forReplay); + const xyz_pos_t l_soft_min = soft_endstop.min.asLogical(), + l_soft_max = soft_endstop.max.asLogical(); + print_pos(l_soft_min, F(STR_SOFT_MIN), F(" ")); + print_pos(l_soft_max, F(STR_SOFT_MAX)); +} + +#endif // HAS_SOFTWARE_ENDSTOPS diff --git a/src/gcode/control/M226.cpp b/src/gcode/control/M226.cpp new file mode 100644 index 0000000..4eb3db4 --- /dev/null +++ b/src/gcode/control/M226.cpp @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(DIRECT_PIN_CONTROL) + +#include "../gcode.h" +#include "../../MarlinCore.h" // for pin_is_protected and idle() +#include "../../module/planner.h" + +void protected_pin_err(); + +/** + * M226: Wait until the specified pin reaches the state required (M226 P S) + */ +void GcodeSuite::M226() { + if (parser.seen('P')) { + const int pin_number = PARSED_PIN_INDEX('P', 0), + pin_state = parser.intval('S', -1); // required pin state - default is inverted + const pin_t pin = GET_PIN_MAP_PIN(pin_number); + + if (WITHIN(pin_state, -1, 1) && pin > -1) { + if (pin_is_protected(pin)) + protected_pin_err(); + else { + int target = LOW; + planner.synchronize(); + pinMode(pin, INPUT); + switch (pin_state) { + case 1: target = HIGH; break; + case 0: target = LOW; break; + case -1: target = !extDigitalRead(pin); break; + } + while (int(extDigitalRead(pin)) != target) idle(); + } + } // pin_state -1 0 1 && pin > -1 + } // parser.seen('P') +} + +#endif // DIRECT_PIN_CONTROL diff --git a/src/gcode/control/M280.cpp b/src/gcode/control/M280.cpp new file mode 100644 index 0000000..82981e4 --- /dev/null +++ b/src/gcode/control/M280.cpp @@ -0,0 +1,76 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_SERVOS + +#include "../gcode.h" +#include "../../module/servo.h" +#include "../../module/planner.h" + +/** + * M280: Get or set servo position. + * P - Servo index + * S - Angle to set, omit to read current angle, or use -1 to detach + * + * With POLARGRAPH: + * T - Duration of servo move + */ +void GcodeSuite::M280() { + + if (!parser.seenval('P')) return; + + TERN_(POLARGRAPH, planner.synchronize()); + + const int servo_index = parser.value_int(); + if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) { + if (parser.seenval('S')) { + const int anew = parser.value_int(); + if (anew >= 0) { + #if ENABLED(POLARGRAPH) + if (parser.seenval('T')) { // (ms) Total duration of servo move + const int16_t t = constrain(parser.value_int(), 0, 10000); + const int aold = servo[servo_index].read(); + millis_t now = millis(); + const millis_t start = now, end = start + t; + while (PENDING(now, end)) { + safe_delay(50); + now = _MIN(millis(), end); + servo[servo_index].move(LROUND(aold + (anew - aold) * (float(now - start) / t))); + } + } + #endif // POLARGRAPH + servo[servo_index].move(anew); + } + else + servo[servo_index].detach(); + } + else + SERIAL_ECHO_MSG(" Servo ", servo_index, ": ", servo[servo_index].read()); + } + else + SERIAL_ERROR_MSG("Servo ", servo_index, " out of range"); + +} + +#endif // HAS_SERVOS diff --git a/src/gcode/control/M282.cpp b/src/gcode/control/M282.cpp new file mode 100644 index 0000000..3ac5ac9 --- /dev/null +++ b/src/gcode/control/M282.cpp @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SERVO_DETACH_GCODE) + +#include "../gcode.h" +#include "../../module/servo.h" + +/** + * M282: Detach Servo. P + */ +void GcodeSuite::M282() { + + if (!parser.seenval('P')) return; + + const int servo_index = parser.value_int(); + if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) + servo[servo_index].detach(); + else + SERIAL_ECHO_MSG("Servo ", servo_index, " out of range"); + +} + +#endif // SERVO_DETACH_GCODE diff --git a/src/gcode/control/M3-M5.cpp b/src/gcode/control/M3-M5.cpp new file mode 100644 index 0000000..5d5d44e --- /dev/null +++ b/src/gcode/control/M3-M5.cpp @@ -0,0 +1,156 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_CUTTER + +#include "../gcode.h" +#include "../../feature/spindle_laser.h" +#include "../../module/planner.h" + +/** + * Laser: + * M3 - Laser ON/Power (Ramped power) + * M4 - Laser ON/Power (Ramped power) + * M5 - Set power output to 0 (leaving inline mode unchanged). + * + * M3I - Enable continuous inline power to be processed by the planner, with power + * calculated and set in the planner blocks, processed inline during stepping. + * Within inline mode M3 S-Values will set the power for the next moves e.g. G1 X10 Y10 powers on with the last S-Value. + * M3I must be set before using planner-synced M3 inline S-Values (LASER_POWER_SYNC). + * + * M4I - Set dynamic mode which calculates laser power OCR based on the current feedrate. + * + * M5I - Clear inline mode and set power to 0. + * + * Spindle: + * M3 - Spindle ON (Clockwise) + * M4 - Spindle ON (Counter-clockwise) + * M5 - Spindle OFF + * + * Parameters: + * S - Set power. S0 will turn the spindle/laser off. + * + * If no PWM pin is defined then M3/M4 just turns it on or off. + * + * At least 12.8kHz (50Hz * 256) is needed for Spindle PWM. + * Hardware PWM is required on AVR. ISRs are too slow. + * + * NOTE: WGM for timers 3, 4, and 5 must be either Mode 1 or Mode 5. + * No other settings give a PWM signal that goes from 0 to 5 volts. + * + * The system automatically sets WGM to Mode 1, so no special + * initialization is needed. + * + * WGM bits for timer 2 are automatically set by the system to + * Mode 1. This produces an acceptable 0 to 5 volt signal. + * No special initialization is needed. + * + * NOTE: A minimum PWM frequency of 50 Hz is needed. All prescaler + * factors for timers 2, 3, 4, and 5 are acceptable. + * + * SPINDLE_LASER_ENA_PIN needs an external pullup or it may power on + * the spindle/laser during power-up or when connecting to the host + * (usually goes through a reset which sets all I/O pins to tri-state) + * + * PWM duty cycle goes from 0 (off) to 255 (always on). + */ +void GcodeSuite::M3_M4(const bool is_M4) { + #if LASER_SAFETY_TIMEOUT_MS > 0 + reset_stepper_timeout(); // Reset timeout to allow subsequent G-code to power the laser (imm.) + #endif + + if (cutter.cutter_mode == CUTTER_MODE_STANDARD) + planner.synchronize(); // Wait for previous movement commands (G0/G1/G2/G3) to complete before changing power + + #if ENABLED(LASER_FEATURE) + if (parser.seen_test('I')) { + cutter.cutter_mode = is_M4 ? CUTTER_MODE_DYNAMIC : CUTTER_MODE_CONTINUOUS; + cutter.inline_power(0); + cutter.set_enabled(true); + } + #endif + + auto get_s_power = [] { + float u; + if (parser.seenval('S')) { + const float v = parser.value_float(); + u = TERN(LASER_POWER_TRAP, v, cutter.power_to_range(v)); + } + else if (cutter.cutter_mode == CUTTER_MODE_STANDARD) + u = cutter.cpwr_to_upwr(SPEED_POWER_STARTUP); + + cutter.menuPower = cutter.unitPower = u; + + // PWM not implied, power converted to OCR from unit definition and on/off if not PWM. + cutter.power = TERN(SPINDLE_LASER_USE_PWM, cutter.upower_to_ocr(u), u > 0 ? 255 : 0); + return u; + }; + + if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS || cutter.cutter_mode == CUTTER_MODE_DYNAMIC) { // Laser power in inline mode + #if ENABLED(LASER_FEATURE) + planner.laser_inline.status.isPowered = true; // M3 or M4 is powered either way + get_s_power(); // Update cutter.power if seen + #if ENABLED(LASER_POWER_SYNC) + // With power sync we only set power so it does not effect queued inline power sets + planner.buffer_sync_block(BLOCK_BIT_LASER_PWR); // Send the flag, queueing inline power + #else + planner.synchronize(); + cutter.inline_power(cutter.power); + #endif + #endif + } + else { + cutter.set_enabled(true); + get_s_power(); + cutter.apply_power( + #if ENABLED(SPINDLE_SERVO) + cutter.unitPower + #elif ENABLED(SPINDLE_LASER_USE_PWM) + cutter.upower_to_ocr(cutter.unitPower) + #else + cutter.unitPower > 0 ? 255 : 0 + #endif + ); + TERN_(SPINDLE_CHANGE_DIR, cutter.set_reverse(is_M4)); + } +} + +/** + * M5 - Cutter OFF (when moves are complete) + */ +void GcodeSuite::M5() { + planner.synchronize(); + cutter.power = 0; + cutter.apply_power(0); // M5 just kills power, leaving inline mode unchanged + if (cutter.cutter_mode != CUTTER_MODE_STANDARD) { + if (parser.seen_test('I')) { + TERN_(LASER_FEATURE, cutter.inline_power(cutter.power)); + cutter.set_enabled(false); // Needs to happen while we are in inline mode to clear inline power. + cutter.cutter_mode = CUTTER_MODE_STANDARD; // Switch from inline to standard mode. + } + } + cutter.set_enabled(false); // Disable enable output setting +} + +#endif // HAS_CUTTER diff --git a/src/gcode/control/M350_M351.cpp b/src/gcode/control/M350_M351.cpp new file mode 100644 index 0000000..ac6b5a3 --- /dev/null +++ b/src/gcode/control/M350_M351.cpp @@ -0,0 +1,74 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_MICROSTEPS + +#include "../gcode.h" +#include "../../module/stepper.h" + +#if NUM_AXES == XYZ && EXTRUDERS >= 1 + #define HAS_M350_B_PARAM 1 // "5th axis" (after E0) for an original XYZEB setup. + #if AXIS_COLLISION('B') + #error "M350 parameter 'B' collision with axis name." + #endif +#endif + +/** + * M350: Set axis microstepping modes. S sets mode for all drivers. + * + * Warning: Steps-per-unit remains unchanged. + */ +void GcodeSuite::M350() { + if (parser.seen('S')) LOOP_DISTINCT_AXES(i) stepper.microstep_mode(i, parser.value_byte()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(AXIS_CHAR(i))) stepper.microstep_mode(i, parser.value_byte()); + TERN_(HAS_M350_B_PARAM, if (parser.seenval('B')) stepper.microstep_mode(E_AXIS + 1, parser.value_byte())); + stepper.microstep_readings(); +} + +/** + * M351: Toggle MS1 MS2 pins directly with axis codes X Y Z . . . E [B] + * S# determines MS1, MS2 or MS3, X# sets the pin high/low. + * + * Parameter 'B' sets "5th axis" (after E0) only for an original XYZEB setup. + */ +void GcodeSuite::M351() { + const int8_t bval = TERN(HAS_M350_B_PARAM, parser.byteval('B', -1), -1); UNUSED(bval); + if (parser.seenval('S')) switch (parser.value_byte()) { + case 1: + LOOP_LOGICAL_AXES(i) if (parser.seenval(AXIS_CHAR(i))) stepper.microstep_ms(i, parser.value_byte(), -1, -1); + TERN_(HAS_M350_B_PARAM, if (bval >= 0) stepper.microstep_ms(E_AXIS + 1, bval != 0, -1, -1)); + break; + case 2: + LOOP_LOGICAL_AXES(i) if (parser.seenval(AXIS_CHAR(i))) stepper.microstep_ms(i, -1, parser.value_byte(), -1); + TERN_(HAS_M350_B_PARAM, if (bval >= 0) stepper.microstep_ms(E_AXIS + 1, -1, bval != 0, -1)); + break; + case 3: + LOOP_LOGICAL_AXES(i) if (parser.seenval(AXIS_CHAR(i))) stepper.microstep_ms(i, -1, -1, parser.value_byte()); + TERN_(HAS_M350_B_PARAM, if (bval >= 0) stepper.microstep_ms(E_AXIS + 1, -1, -1, bval != 0)); + break; + } + stepper.microstep_readings(); +} + +#endif // HAS_MICROSTEPS diff --git a/src/gcode/control/M380_M381.cpp b/src/gcode/control/M380_M381.cpp new file mode 100644 index 0000000..6bcec89 --- /dev/null +++ b/src/gcode/control/M380_M381.cpp @@ -0,0 +1,56 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if EITHER(EXT_SOLENOID, MANUAL_SOLENOID_CONTROL) + +#include "../gcode.h" +#include "../../feature/solenoid.h" +#include "../../module/motion.h" + +/** + * M380: Enable solenoid on the active extruder + * + * S to specify a solenoid (Requires MANUAL_SOLENOID_CONTROL) + */ +void GcodeSuite::M380() { + #if ENABLED(MANUAL_SOLENOID_CONTROL) + enable_solenoid(parser.intval('S', active_extruder)); + #else + enable_solenoid(active_extruder); + #endif +} + +/** + * M381: Disable all solenoids if EXT_SOLENOID + * Disable selected/active solenoid if MANUAL_SOLENOID_CONTROL + */ +void GcodeSuite::M381() { + #if ENABLED(MANUAL_SOLENOID_CONTROL) + disable_solenoid(parser.intval('S', active_extruder)); + #else + disable_all_solenoids(); + #endif +} + +#endif // EXT_SOLENOID || MANUAL_SOLENOID_CONTROL diff --git a/src/gcode/control/M400.cpp b/src/gcode/control/M400.cpp new file mode 100644 index 0000000..6058fb8 --- /dev/null +++ b/src/gcode/control/M400.cpp @@ -0,0 +1,33 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/planner.h" + +/** + * M400: Finish all moves + */ +void GcodeSuite::M400() { + + planner.synchronize(); + +} diff --git a/src/gcode/control/M42.cpp b/src/gcode/control/M42.cpp new file mode 100644 index 0000000..1b3a29d --- /dev/null +++ b/src/gcode/control/M42.cpp @@ -0,0 +1,135 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(DIRECT_PIN_CONTROL) + +#include "../gcode.h" +#include "../../MarlinCore.h" // for pin_is_protected + +#if HAS_FAN + #include "../../module/temperature.h" +#endif + +#ifdef MAPLE_STM32F1 + // these are enums on the F1... + #define INPUT_PULLDOWN INPUT_PULLDOWN + #define INPUT_ANALOG INPUT_ANALOG + #define OUTPUT_OPEN_DRAIN OUTPUT_OPEN_DRAIN +#endif + +void protected_pin_err() { + SERIAL_ERROR_MSG(STR_ERR_PROTECTED_PIN); +} + +/** + * M42: Change pin status via GCode + * + * P Pin number (LED if omitted) + * For LPC1768 specify pin P1_02 as M42 P102, + * P1_20 as M42 P120, etc. + * + * S Pin status from 0 - 255 + * I Flag to ignore Marlin's pin protection + * + * T Pin mode: 0=INPUT 1=OUTPUT 2=INPUT_PULLUP 3=INPUT_PULLDOWN + */ +void GcodeSuite::M42() { + const int pin_index = PARSED_PIN_INDEX('P', GET_PIN_MAP_INDEX(LED_PIN)); + if (pin_index < 0) return; + + const pin_t pin = GET_PIN_MAP_PIN(pin_index); + + if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err(); + + bool avoidWrite = false; + if (parser.seenval('T')) { + switch (parser.value_byte()) { + case 0: pinMode(pin, INPUT); avoidWrite = true; break; + case 1: pinMode(pin, OUTPUT); break; + case 2: pinMode(pin, INPUT_PULLUP); avoidWrite = true; break; + #ifdef INPUT_PULLDOWN + case 3: pinMode(pin, INPUT_PULLDOWN); avoidWrite = true; break; + #endif + #ifdef INPUT_ANALOG + case 4: pinMode(pin, INPUT_ANALOG); avoidWrite = true; break; + #endif + #ifdef OUTPUT_OPEN_DRAIN + case 5: pinMode(pin, OUTPUT_OPEN_DRAIN); break; + #endif + default: SERIAL_ECHOLNPGM("Invalid Pin Mode"); return; + } + } + + if (!parser.seenval('S')) return; + const byte pin_status = parser.value_byte(); + + #if HAS_FAN + switch (pin) { + #if HAS_FAN0 + case FAN0_PIN: thermalManager.fan_speed[0] = pin_status; return; + #endif + #if HAS_FAN1 + case FAN1_PIN: thermalManager.fan_speed[1] = pin_status; return; + #endif + #if HAS_FAN2 + case FAN2_PIN: thermalManager.fan_speed[2] = pin_status; return; + #endif + #if HAS_FAN3 + case FAN3_PIN: thermalManager.fan_speed[3] = pin_status; return; + #endif + #if HAS_FAN4 + case FAN4_PIN: thermalManager.fan_speed[4] = pin_status; return; + #endif + #if HAS_FAN5 + case FAN5_PIN: thermalManager.fan_speed[5] = pin_status; return; + #endif + #if HAS_FAN6 + case FAN6_PIN: thermalManager.fan_speed[6] = pin_status; return; + #endif + #if HAS_FAN7 + case FAN7_PIN: thermalManager.fan_speed[7] = pin_status; return; + #endif + } + #endif + + if (avoidWrite) { + SERIAL_ECHOLNPGM("?Cannot write to INPUT"); + return; + } + + // An OUTPUT_OPEN_DRAIN should not be changed to normal OUTPUT (STM32) + // Use M42 Px M1/5 S0/1 to set the output type and then set value + #ifndef OUTPUT_OPEN_DRAIN + pinMode(pin, OUTPUT); + #endif + extDigitalWrite(pin, pin_status); + + #ifdef ARDUINO_ARCH_STM32 + // A simple I/O will be set to 0 by hal.set_pwm_duty() + if (pin_status <= 1 && !PWM_PIN(pin)) return; + #endif + hal.set_pwm_duty(pin, pin_status); +} + +#endif // DIRECT_PIN_CONTROL diff --git a/src/gcode/control/M605.cpp b/src/gcode/control/M605.cpp new file mode 100644 index 0000000..e3ca43e --- /dev/null +++ b/src/gcode/control/M605.cpp @@ -0,0 +1,189 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_DUPLICATION_MODE + +//#define DEBUG_DXC_MODE + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/tool_change.h" +#include "../../module/planner.h" + +#define DEBUG_OUT ENABLED(DEBUG_DXC_MODE) +#include "../../core/debug_out.h" + +#if ENABLED(DUAL_X_CARRIAGE) + + /** + * M605: Set dual x-carriage movement mode + * + * M605 S0 : (FULL_CONTROL) The slicer has full control over both X-carriages and can achieve optimal travel + * results as long as it supports dual X-carriages. + * + * M605 S1 : (AUTO_PARK) The firmware automatically parks and unparks the X-carriages on tool-change so that + * additional slicer support is not required. + * + * M605 S2 X R : (DUPLICATION) The firmware moves the second X-carriage and extruder in synchronization with + * the first X-carriage and extruder, to print 2 copies of the same object at the same time. + * Set the constant X-offset and temperature differential with M605 S2 X[offs] R[deg] and + * follow with "M605 S2" to initiate duplicated movement. For example, use "M605 S2 X100 R2" to + * make a copy 100mm to the right with E1 2° hotter than E0. + * + * M605 S3 : (MIRRORED) Formbot/Vivedino-inspired mirrored mode in which the second extruder duplicates + * the movement of the first except the second extruder is reversed in the X axis. + * The temperature differential and initial X offset must be set with "M605 S2 X[offs] R[deg]", + * then followed by "M605 S3" to initiate mirrored movement. + * + * M605 W : IDEX What? command. + * + * Note: the X axis should be homed after changing Dual X-carriage mode. + */ + void GcodeSuite::M605() { + planner.synchronize(); + + if (parser.seenval('S')) { + const DualXMode previous_mode = dual_x_carriage_mode; + + dual_x_carriage_mode = (DualXMode)parser.value_byte(); + idex_set_mirrored_mode(false); + + switch (dual_x_carriage_mode) { + + case DXC_FULL_CONTROL_MODE: + case DXC_AUTO_PARK_MODE: + break; + + case DXC_DUPLICATION_MODE: + // Set the X offset, but no less than the safety gap + if (parser.seenval('X')) duplicate_extruder_x_offset = _MAX(parser.value_linear_units(), (X2_MIN_POS) - (X1_MIN_POS)); + if (parser.seenval('R')) duplicate_extruder_temp_offset = parser.value_celsius_diff(); + // Always switch back to tool 0 + if (active_extruder != 0) tool_change(0); + break; + + case DXC_MIRRORED_MODE: { + if (previous_mode != DXC_DUPLICATION_MODE) { + SERIAL_ECHOLNPGM("Printer must be in DXC_DUPLICATION_MODE prior to "); + SERIAL_ECHOLNPGM("specifying DXC_MIRRORED_MODE."); + dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; + return; + } + idex_set_mirrored_mode(true); + + // Do a small 'jog' motion in the X axis + xyze_pos_t dest = current_position; dest.x -= 0.1f; + for (uint8_t i = 2; --i;) { + planner.buffer_line(dest, feedrate_mm_s, 0); + dest.x += 0.1f; + } + } return; + + default: + dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; + break; + } + + idex_set_parked(false); + set_duplication_enabled(false); + + #ifdef EVENT_GCODE_IDEX_AFTER_MODECHANGE + process_subcommands_now(F(EVENT_GCODE_IDEX_AFTER_MODECHANGE)); + #endif + } + else if (!parser.seen('W')) // if no S or W parameter, the DXC mode gets reset to the user's default + dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; + + #ifdef DEBUG_DXC_MODE + + if (parser.seen('W')) { + DEBUG_ECHO_START(); + DEBUG_ECHOPGM("Dual X Carriage Mode "); + switch (dual_x_carriage_mode) { + case DXC_FULL_CONTROL_MODE: DEBUG_ECHOPGM("FULL_CONTROL"); break; + case DXC_AUTO_PARK_MODE: DEBUG_ECHOPGM("AUTO_PARK"); break; + case DXC_DUPLICATION_MODE: DEBUG_ECHOPGM("DUPLICATION"); break; + case DXC_MIRRORED_MODE: DEBUG_ECHOPGM("MIRRORED"); break; + } + DEBUG_ECHOPGM("\nActive Ext: ", active_extruder); + if (!active_extruder_parked) DEBUG_ECHOPGM(" NOT "); + DEBUG_ECHOPGM(" parked."); + DEBUG_ECHOPGM("\nactive_extruder_x_pos: ", current_position.x); + DEBUG_ECHOPGM("\ninactive_extruder_x: ", inactive_extruder_x); + DEBUG_ECHOPGM("\nextruder_duplication_enabled: ", extruder_duplication_enabled); + DEBUG_ECHOPGM("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset); + DEBUG_ECHOPGM("\nduplicate_extruder_temp_offset: ", duplicate_extruder_temp_offset); + DEBUG_ECHOPGM("\ndelayed_move_time: ", delayed_move_time); + DEBUG_ECHOPGM("\nX1 Home X: ", x_home_pos(0), "\nX1_MIN_POS=", X1_MIN_POS, "\nX1_MAX_POS=", X1_MAX_POS); + DEBUG_ECHOPGM("\nX2 Home X: ", x_home_pos(1), "\nX2_MIN_POS=", X2_MIN_POS, "\nX2_MAX_POS=", X2_MAX_POS); + DEBUG_ECHOPGM("\nX2_HOME_DIR=", X2_HOME_DIR, "\nX2_HOME_POS=", X2_HOME_POS); + DEBUG_ECHOPGM("\nDEFAULT_DUAL_X_CARRIAGE_MODE=", STRINGIFY(DEFAULT_DUAL_X_CARRIAGE_MODE)); + DEBUG_ECHOPGM("\toolchange_settings.z_raise=", toolchange_settings.z_raise); + DEBUG_ECHOPGM("\nDEFAULT_DUPLICATION_X_OFFSET=", DEFAULT_DUPLICATION_X_OFFSET); + DEBUG_EOL(); + + HOTEND_LOOP() { + DEBUG_ECHOPGM_P(SP_T_STR, e); + LOOP_NUM_AXES(a) DEBUG_ECHOPGM(" hotend_offset[", e, "].", AS_CHAR(AXIS_CHAR(a) | 0x20), "=", hotend_offset[e][a]); + DEBUG_EOL(); + } + DEBUG_EOL(); + } + #endif // DEBUG_DXC_MODE + } + +#elif ENABLED(MULTI_NOZZLE_DUPLICATION) + + /** + * M605: Set multi-nozzle duplication mode + * + * S2 - Enable duplication mode + * P[mask] - Bit-mask of nozzles to include in the duplication set. + * A value of 0 disables duplication. + * E[index] - Last nozzle index to include in the duplication set. + * A value of 0 disables duplication. + */ + void GcodeSuite::M605() { + bool ena = false; + if (parser.seen("EPS")) { + planner.synchronize(); + if (parser.seenval('P')) duplication_e_mask = parser.value_int(); // Set the mask directly + else if (parser.seenval('E')) duplication_e_mask = pow(2, parser.value_int() + 1) - 1; // Set the mask by E index + ena = (2 == parser.intval('S', extruder_duplication_enabled ? 2 : 0)); + set_duplication_enabled(ena && (duplication_e_mask >= 3)); + } + SERIAL_ECHO_START(); + SERIAL_ECHOPGM(STR_DUPLICATION_MODE); + serialprint_onoff(extruder_duplication_enabled); + if (ena) { + SERIAL_ECHOPGM(" ( "); + HOTEND_LOOP() if (TEST(duplication_e_mask, e)) { SERIAL_ECHO(e); SERIAL_CHAR(' '); } + SERIAL_CHAR(')'); + } + SERIAL_EOL(); + } + +#endif // MULTI_NOZZLE_DUPLICATION + +#endif // HAS_DUPICATION_MODE diff --git a/src/gcode/control/M7-M9.cpp b/src/gcode/control/M7-M9.cpp new file mode 100644 index 0000000..ccde4f5 --- /dev/null +++ b/src/gcode/control/M7-M9.cpp @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ANY(COOLANT_MIST, COOLANT_FLOOD, AIR_ASSIST) + +#include "../gcode.h" +#include "../../module/planner.h" + +#if ENABLED(COOLANT_MIST) + /** + * M7: Mist Coolant On + */ + void GcodeSuite::M7() { + planner.synchronize(); // Wait for move to arrive + WRITE(COOLANT_MIST_PIN, !(COOLANT_MIST_INVERT)); // Turn on Mist coolant + } +#endif + +#if EITHER(COOLANT_FLOOD, AIR_ASSIST) + + #if ENABLED(AIR_ASSIST) + #include "../../feature/spindle_laser.h" + #endif + + /** + * M8: Flood Coolant / Air Assist ON + */ + void GcodeSuite::M8() { + planner.synchronize(); // Wait for move to arrive + #if ENABLED(COOLANT_FLOOD) + WRITE(COOLANT_FLOOD_PIN, !(COOLANT_FLOOD_INVERT)); // Turn on Flood coolant + #endif + #if ENABLED(AIR_ASSIST) + cutter.air_assist_enable(); // Turn on Air Assist + #endif + } + +#endif + +/** + * M9: Coolant / Air Assist OFF + */ +void GcodeSuite::M9() { + planner.synchronize(); // Wait for move to arrive + #if ENABLED(COOLANT_MIST) + WRITE(COOLANT_MIST_PIN, COOLANT_MIST_INVERT); // Turn off Mist coolant + #endif + #if ENABLED(COOLANT_FLOOD) + WRITE(COOLANT_FLOOD_PIN, COOLANT_FLOOD_INVERT); // Turn off Flood coolant + #endif + #if ENABLED(AIR_ASSIST) + cutter.air_assist_disable(); // Turn off Air Assist + #endif +} + +#endif // COOLANT_MIST | COOLANT_FLOOD | AIR_ASSIST diff --git a/src/gcode/control/M80_M81.cpp b/src/gcode/control/M80_M81.cpp new file mode 100644 index 0000000..90b25e7 --- /dev/null +++ b/src/gcode/control/M80_M81.cpp @@ -0,0 +1,120 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" + +#include "../../module/temperature.h" +#include "../../module/planner.h" // for planner.finish_and_disable +#include "../../module/printcounter.h" // for print_job_timer.stop +#include "../../lcd/marlinui.h" // for LCD_MESSAGE_F + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(PSU_CONTROL) + #include "../queue.h" + #include "../../feature/power.h" +#endif + +#if HAS_SUICIDE + #include "../../MarlinCore.h" +#endif + +#if ENABLED(PSU_CONTROL) + + /** + * M80 : Turn on the Power Supply + * M80 S : Report the current state and exit + */ + void GcodeSuite::M80() { + + // S: Report the current power supply state and exit + if (parser.seen('S')) { + SERIAL_ECHOF(powerManager.psu_on ? F("PS:1\n") : F("PS:0\n")); + return; + } + + powerManager.power_on(); + + /** + * If you have a switch on suicide pin, this is useful + * if you want to start another print with suicide feature after + * a print without suicide... + */ + #if HAS_SUICIDE + OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_STATE); + #endif + + TERN_(HAS_MARLINUI_MENU, ui.reset_status()); + } + +#endif // PSU_CONTROL + +/** + * M81: Turn off Power, including Power Supply, if there is one. + * + * This code should ALWAYS be available for FULL SHUTDOWN! + */ +void GcodeSuite::M81() { + planner.finish_and_disable(); + thermalManager.cooldown(); + + print_job_timer.stop(); + + #if BOTH(HAS_FAN, PROBING_FANS_OFF) + thermalManager.fans_paused = false; + ZERO(thermalManager.saved_fan_speed); + #endif + + safe_delay(1000); // Wait 1 second before switching off + + LCD_MESSAGE_F(MACHINE_NAME " " STR_OFF "."); + + bool delayed_power_off = false; + + #if ENABLED(POWER_OFF_TIMER) + if (parser.seenval('D')) { + uint16_t delay = parser.value_ushort(); + if (delay > 1) { // skip already observed 1s delay + delayed_power_off = true; + powerManager.setPowerOffTimer(SEC_TO_MS(delay - 1)); + } + } + #endif + + #if ENABLED(POWER_OFF_WAIT_FOR_COOLDOWN) + if (parser.boolval('S')) { + delayed_power_off = true; + powerManager.setPowerOffOnCooldown(true); + } + #endif + + if (delayed_power_off) { + SERIAL_ECHOLNPGM(STR_DELAYED_POWEROFF); + return; + } + + #if HAS_SUICIDE + suicide(); + #elif ENABLED(PSU_CONTROL) + powerManager.power_off_soon(); + #endif +} diff --git a/src/gcode/control/M85.cpp b/src/gcode/control/M85.cpp new file mode 100644 index 0000000..ee86834 --- /dev/null +++ b/src/gcode/control/M85.cpp @@ -0,0 +1,42 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" + +/** + * M85: Set inactivity shutdown timer with parameter S. To disable set zero (default) + */ +void GcodeSuite::M85() { + + if (parser.seen('S')) { + reset_stepper_timeout(); + const millis_t ms = parser.value_millis_from_seconds(); + #if LASER_SAFETY_TIMEOUT_MS > 0 + if (ms && ms <= LASER_SAFETY_TIMEOUT_MS) { + SERIAL_ECHO_MSG("M85 timeout must be > ", MS_TO_SEC(LASER_SAFETY_TIMEOUT_MS + 999), " s for laser safety."); + return; + } + #endif + max_inactive_time = ms; + } + +} diff --git a/src/gcode/control/M993_M994.cpp b/src/gcode/control/M993_M994.cpp new file mode 100644 index 0000000..252792e --- /dev/null +++ b/src/gcode/control/M993_M994.cpp @@ -0,0 +1,88 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ALL(HAS_SPI_FLASH, SDSUPPORT, MARLIN_DEV_MODE) + +#include "../gcode.h" +#include "../../sd/cardreader.h" +#include "../../libs/W25Qxx.h" + +/** + * M993: Backup SPI Flash to SD + */ +void GcodeSuite::M993() { + if (!card.isMounted()) card.mount(); + + char fname[] = "spiflash.bin"; + card.openFileWrite(fname); + if (!card.isFileOpen()) { + SERIAL_ECHOLNPGM("Failed to open ", fname, " to write."); + return; + } + + uint8_t buf[1024]; + uint32_t addr = 0; + W25QXX.init(SPI_QUARTER_SPEED); + SERIAL_ECHOPGM("Save SPI Flash"); + while (addr < SPI_FLASH_SIZE) { + W25QXX.SPI_FLASH_BufferRead(buf, addr, COUNT(buf)); + addr += COUNT(buf); + card.write(buf, COUNT(buf)); + if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.'); + } + SERIAL_ECHOLNPGM(" done"); + + card.closefile(); +} + +/** + * M994: Load a backup from SD to SPI Flash + */ +void GcodeSuite::M994() { + if (!card.isMounted()) card.mount(); + + char fname[] = "spiflash.bin"; + card.openFileRead(fname); + if (!card.isFileOpen()) { + SERIAL_ECHOLNPGM("Failed to open ", fname, " to read."); + return; + } + + uint8_t buf[1024]; + uint32_t addr = 0; + W25QXX.init(SPI_QUARTER_SPEED); + W25QXX.SPI_FLASH_BulkErase(); + SERIAL_ECHOPGM("Load SPI Flash"); + while (addr < SPI_FLASH_SIZE) { + card.read(buf, COUNT(buf)); + W25QXX.SPI_FLASH_BufferWrite(buf, addr, COUNT(buf)); + addr += COUNT(buf); + if (addr % (COUNT(buf) * 10) == 0) SERIAL_CHAR('.'); + } + SERIAL_ECHOLNPGM(" done"); + + card.closefile(); +} + +#endif // HAS_SPI_FLASH && SDSUPPORT && MARLIN_DEV_MODE diff --git a/src/gcode/control/M997.cpp b/src/gcode/control/M997.cpp new file mode 100644 index 0000000..74ed8b0 --- /dev/null +++ b/src/gcode/control/M997.cpp @@ -0,0 +1,42 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" + +#if ENABLED(PLATFORM_M997_SUPPORT) + +#if ENABLED(DWIN_LCD_PROUI) + #include "../../lcd/e3v2/proui/dwin.h" +#endif + +/** + * M997: Perform in-application firmware update + */ +void GcodeSuite::M997() { + + TERN_(DWIN_LCD_PROUI, DWIN_RebootScreen()); + + flashFirmware(parser.intval('S')); + +} + +#endif diff --git a/src/gcode/control/M999.cpp b/src/gcode/control/M999.cpp new file mode 100644 index 0000000..b7d6db9 --- /dev/null +++ b/src/gcode/control/M999.cpp @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" + +#include "../../lcd/marlinui.h" // for ui.reset_alert_level +#include "../../MarlinCore.h" // for marlin_state +#include "../queue.h" // for flush_and_request_resend + +/** + * M999: Restart after being stopped + * + * Default behavior is to flush the serial buffer and request + * a resend to the host starting on the last N line received. + * + * Sending "M999 S1" will resume printing without flushing the + * existing command buffer. + */ +void GcodeSuite::M999() { + marlin_state = MF_RUNNING; + ui.reset_alert_level(); + + if (parser.boolval('S')) return; + + queue.flush_and_request_resend(queue.ring_buffer.command_port()); +} diff --git a/src/gcode/control/T.cpp b/src/gcode/control/T.cpp new file mode 100644 index 0000000..5e8f6b5 --- /dev/null +++ b/src/gcode/control/T.cpp @@ -0,0 +1,70 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/tool_change.h" + +#if EITHER(HAS_MULTI_EXTRUDER, DEBUG_LEVELING_FEATURE) + #include "../../module/motion.h" +#endif + +#if HAS_PRUSA_MMU2 + #include "../../feature/mmu/mmu2.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" + +/** + * T0-T: Switch tool, usually switching extruders + * + * F[units/min] Set the movement feedrate + * S1 Don't move the tool in XY after change + * + * For PRUSA_MMU2(S) and EXTENDABLE_EMU_MMU2(S) + * T[n] Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels. + * T? Gcode to extrude shouldn't have to follow. Load to extruder wheels is done automatically. + * Tx Same as T?, but nozzle doesn't have to be preheated. Tc requires a preheated nozzle to finish filament load. + * Tc Load to nozzle after filament was prepared by Tc and nozzle is already heated. + */ +void GcodeSuite::T(const int8_t tool_index) { + + DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING)); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("...(", tool_index, ")"); + + // Count this command as movement / activity + reset_stepper_timeout(); + + #if HAS_PRUSA_MMU2 + if (parser.string_arg) { + mmu2.tool_change(parser.string_arg); // Special commands T?/Tx/Tc + return; + } + #endif + + tool_change(tool_index + #if HAS_MULTI_EXTRUDER + , TERN(PARKING_EXTRUDER, false, tool_index == active_extruder) // For PARKING_EXTRUDER motion is decided in tool_change() + || parser.boolval('S') + #endif + ); +} diff --git a/src/gcode/eeprom/M500-M504.cpp b/src/gcode/eeprom/M500-M504.cpp new file mode 100644 index 0000000..412d003 --- /dev/null +++ b/src/gcode/eeprom/M500-M504.cpp @@ -0,0 +1,124 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../gcode.h" +#include "../../module/settings.h" +#include "../../core/serial.h" +#include "../../inc/MarlinConfig.h" + +#if ENABLED(CONFIGURATION_EMBEDDING) + #include "../../sd/SdBaseFile.h" + #include "../../mczip.h" +#endif + +/** + * M500: Store settings in EEPROM + */ +void GcodeSuite::M500() { + (void)settings.save(); +} + +/** + * M501: Read settings from EEPROM + */ +void GcodeSuite::M501() { + (void)settings.load(); +} + +/** + * M502: Revert to default settings + */ +void GcodeSuite::M502() { + (void)settings.reset(); +} + +#if DISABLED(DISABLE_M503) + + /** + * M503: print settings currently in memory + * + * S : Include / exclude header comments in the output. (Default: S1) + * + * With CONFIGURATION_EMBEDDING: + * C : Save the full Marlin configuration to SD Card as "mc.zip" + */ + void GcodeSuite::M503() { + (void)settings.report(!parser.boolval('S', true)); + + #if ENABLED(CONFIGURATION_EMBEDDING) + if (parser.seen_test('C')) { + SdBaseFile file; + const uint16_t size = sizeof(mc_zip); + // Need to create the config size on the SD card + if (file.open("mc.zip", O_WRITE|O_CREAT) && file.write(pgm_read_ptr(mc_zip), size) != -1 && file.close()) + SERIAL_ECHO_MSG("Configuration saved as 'mc.zip'"); + } + #endif + } + +#endif // !DISABLE_M503 + +#if ENABLED(EEPROM_SETTINGS) + + #if ENABLED(MARLIN_DEV_MODE) + #include "../../libs/hex_print.h" + #endif + + /** + * M504: Validate EEPROM Contents + */ + void GcodeSuite::M504() { + #if ENABLED(MARLIN_DEV_MODE) + const bool dowrite = parser.seenval('W'); + if (dowrite || parser.seenval('R')) { + uint8_t val = 0; + int addr = parser.value_ushort(); + if (dowrite) { + val = parser.byteval('V'); + persistentStore.write_data(addr, &val); + SERIAL_ECHOLNPGM("Wrote address ", addr, " with ", val); + } + else { + if (parser.seenval('T')) { + const int endaddr = parser.value_ushort(); + while (addr <= endaddr) { + persistentStore.read_data(addr, &val); + SERIAL_ECHOLNPGM("0x", hex_word(addr), ":", hex_byte(val)); + addr++; + safe_delay(10); + } + SERIAL_EOL(); + } + else { + persistentStore.read_data(addr, &val); + SERIAL_ECHOLNPGM("Read address ", addr, " and got ", val); + } + } + return; + } + #endif + + if (settings.validate()) + SERIAL_ECHO_MSG("EEPROM OK"); + } + +#endif diff --git a/src/gcode/feature/L6470/M122.cpp b/src/gcode/feature/L6470/M122.cpp new file mode 100644 index 0000000..4a5629b --- /dev/null +++ b/src/gcode/feature/L6470/M122.cpp @@ -0,0 +1,151 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_L64XX + +#include "../../gcode.h" +#include "../../../libs/L64XX/L64XX_Marlin.h" +#include "../../../module/stepper/indirection.h" + +void echo_yes_no(const bool yes); + +inline void L6470_say_status(const L64XX_axis_t axis) { + if (L64xxManager.spi_abort) return; + const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow; + L64xxManager.get_status(axis); + L64xxManager.say_axis(axis); + #if ENABLED(L6470_CHITCHAT) + char temp_buf[20]; + sprintf_P(temp_buf, PSTR(" status: %4x "), sh.STATUS_AXIS_RAW); + SERIAL_ECHO(temp_buf); + print_bin(sh.STATUS_AXIS_RAW); + switch (sh.STATUS_AXIS_LAYOUT) { + case L6470_STATUS_LAYOUT: SERIAL_ECHOPGM(" L6470"); break; + case L6474_STATUS_LAYOUT: SERIAL_ECHOPGM(" L6474"); break; + case L6480_STATUS_LAYOUT: SERIAL_ECHOPGM(" L6480/powerSTEP01"); break; + } + #endif + SERIAL_ECHOPGM("\n...OUTPUT: "); + SERIAL_ECHOF(sh.STATUS_AXIS & STATUS_HIZ ? F("OFF") : F("ON ")); + SERIAL_ECHOPGM(" BUSY: "); echo_yes_no((sh.STATUS_AXIS & STATUS_BUSY) == 0); + SERIAL_ECHOPGM(" DIR: "); + SERIAL_ECHOF((((sh.STATUS_AXIS & STATUS_DIR) >> 4) ^ L64xxManager.index_to_dir[axis]) ? F("FORWARD") : F("REVERSE")); + if (sh.STATUS_AXIS_LAYOUT == L6480_STATUS_LAYOUT) { + SERIAL_ECHOPGM(" Last Command: "); + if (sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD) SERIAL_ECHOPGM("VALID"); + else SERIAL_ECHOPGM("ERROR"); + SERIAL_ECHOPGM("\n...THERMAL: "); + switch ((sh.STATUS_AXIS & (sh.STATUS_AXIS_TH_SD | sh.STATUS_AXIS_TH_WRN)) >> 11) { + case 0: SERIAL_ECHOPGM("DEVICE SHUTDOWN"); break; + case 1: SERIAL_ECHOPGM("BRIDGE SHUTDOWN"); break; + case 2: SERIAL_ECHOPGM("WARNING "); break; + case 3: SERIAL_ECHOPGM("OK "); break; + } + } + else { + SERIAL_ECHOPGM(" Last Command: "); + if (!(sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD)) SERIAL_ECHOPGM("IN"); + SERIAL_ECHOPGM("VALID "); + SERIAL_ECHOF(sh.STATUS_AXIS & sh.STATUS_AXIS_NOTPERF_CMD ? F("COMPLETED ") : F("Not PERFORMED")); + SERIAL_ECHOPGM("\n...THERMAL: ", !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_SD) ? "SHUTDOWN " : !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_WRN) ? "WARNING " : "OK "); + } + SERIAL_ECHOPGM(" OVERCURRENT:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_OCD) == 0); + if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) { + SERIAL_ECHOPGM(" STALL:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_A) == 0 || (sh.STATUS_AXIS & sh.STATUS_AXIS_STEP_LOSS_B) == 0); + SERIAL_ECHOPGM(" STEP-CLOCK MODE:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_SCK_MOD) != 0); + } + else { + SERIAL_ECHOPGM(" STALL: NA " + " STEP-CLOCK MODE: NA" + " UNDER VOLTAGE LOCKOUT: "); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_UVLO) == 0); + } + SERIAL_EOL(); +} + +/** + * M122: Debug L6470 drivers + */ +void GcodeSuite::M122() { + L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status + L64xxManager.spi_active = true; // Tell set_directions() a series of SPI transfers is underway + + //if (parser.seen('S')) + // tmc_set_report_interval(parser.value_bool()); + //else + + #if AXIS_IS_L64XX(X) + L6470_say_status(X); + #endif + #if AXIS_IS_L64XX(X2) + L6470_say_status(X2); + #endif + #if AXIS_IS_L64XX(Y) + L6470_say_status(Y); + #endif + #if AXIS_IS_L64XX(Y2) + L6470_say_status(Y2); + #endif + #if AXIS_IS_L64XX(Z) + L6470_say_status(Z); + #endif + #if AXIS_IS_L64XX(Z2) + L6470_say_status(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + L6470_say_status(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + L6470_say_status(Z4); + #endif + #if AXIS_IS_L64XX(E0) + L6470_say_status(E0); + #endif + #if AXIS_IS_L64XX(E1) + L6470_say_status(E1); + #endif + #if AXIS_IS_L64XX(E2) + L6470_say_status(E2); + #endif + #if AXIS_IS_L64XX(E3) + L6470_say_status(E3); + #endif + #if AXIS_IS_L64XX(E4) + L6470_say_status(E4); + #endif + #if AXIS_IS_L64XX(E5) + L6470_say_status(E5); + #endif + #if AXIS_IS_L64XX(E6) + L6470_say_status(E6); + #endif + #if AXIS_IS_L64XX(E7) + L6470_say_status(E7); + #endif + + L64xxManager.spi_active = false; // done with all SPI transfers - clear handshake flags + L64xxManager.spi_abort = false; + L64xxManager.pause_monitor(false); +} + +#endif // HAS_L64XX diff --git a/src/gcode/feature/L6470/M906.cpp b/src/gcode/feature/L6470/M906.cpp new file mode 100644 index 0000000..ee0211d --- /dev/null +++ b/src/gcode/feature/L6470/M906.cpp @@ -0,0 +1,380 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_L64XX + +#if AXIS_COLLISION('I') + #error "M906 parameter 'I' collision with axis name." +#endif + +#include "../../gcode.h" +#include "../../../libs/L64XX/L64XX_Marlin.h" +#include "../../../module/stepper/indirection.h" +#include "../../../module/planner.h" + +#define DEBUG_OUT ENABLED(L6470_CHITCHAT) +#include "../../../core/debug_out.h" + +/** + * MACRO to fetch information on the items associated with current limiting + * and maximum voltage output. + * + * L6470 can be setup to shutdown if either current threshold is exceeded. + * + * L6470 output current can not be set directly. It is set indirectly by + * setting the maximum effective output voltage. + * + * Effective output voltage is set by PWM duty cycle. + * + * Maximum effective output voltage is affected by MANY variables. The main ones are: + * KVAL_HOLD + * KVAL_RUN + * KVAL_ACC + * KVAL_DEC + * Vs compensation (if enabled) + */ +void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { + + if (L64xxManager.spi_abort) return; // don't do anything if set_directions() has occurred + + const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow; + const uint16_t status = L64xxManager.get_status(axis); //also populates shadow structure + const uint8_t OverCurrent_Threshold = uint8_t(motor.GetParam(L6470_OCD_TH)); + + auto say_axis_status = [](const L64XX_axis_t axis, const uint16_t status) { + L64xxManager.say_axis(axis); + #if ENABLED(L6470_CHITCHAT) + char tmp[10]; + sprintf_P(tmp, PSTR("%4x "), status); + DEBUG_ECHOPGM(" status: ", tmp); + print_bin(status); + #else + UNUSED(status); + #endif + SERIAL_EOL(); + }; + + char temp_buf[10]; + + switch (sh.STATUS_AXIS_LAYOUT) { + case L6470_STATUS_LAYOUT: // L6470 + case L6480_STATUS_LAYOUT: { // L6480 & powerstep01 + const uint16_t Stall_Threshold = (uint8_t)motor.GetParam(L6470_STALL_TH), + motor_status = (status & (STATUS_MOT_STATUS)) >> 5, + L6470_ADC_out = motor.GetParam(L6470_ADC_OUT), + L6470_ADC_out_limited = constrain(L6470_ADC_out, 8, 24); + const float comp_coef = 1600.0f / L6470_ADC_out_limited; + const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07); + + say_axis_status(axis, sh.STATUS_AXIS_RAW); + + SERIAL_ECHOPGM("...OverCurrent Threshold: "); + sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold); + SERIAL_ECHO(temp_buf); + SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV); + SERIAL_ECHOPGM(" mA)"); + SERIAL_ECHOPGM(" Stall Threshold: "); + sprintf_P(temp_buf, PSTR("%2d ("), Stall_Threshold); + SERIAL_ECHO(temp_buf); + SERIAL_ECHO((Stall_Threshold + 1) * motor.STALL_CURRENT_CONSTANT_INV); + SERIAL_ECHOPGM(" mA)"); + SERIAL_ECHOPGM(" Motor Status: "); + switch (motor_status) { + case 0: SERIAL_ECHOPGM("stopped"); break; + case 1: SERIAL_ECHOPGM("accelerating"); break; + case 2: SERIAL_ECHOPGM("decelerating"); break; + case 3: SERIAL_ECHOPGM("at constant speed"); break; + } + SERIAL_EOL(); + + SERIAL_ECHOPGM("...MicroSteps: ", MicroSteps, + " ADC_OUT: ", L6470_ADC_out); + SERIAL_ECHOPGM(" Vs_compensation: "); + SERIAL_ECHOF((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_EN_VSCOMP) ? F("ENABLED ") : F("DISABLED")); + SERIAL_ECHOLNPGM(" Compensation coefficient: ~", comp_coef * 0.01f); + + SERIAL_ECHOPGM("...KVAL_HOLD: ", motor.GetParam(L6470_KVAL_HOLD), + " KVAL_RUN : ", motor.GetParam(L6470_KVAL_RUN), + " KVAL_ACC: ", motor.GetParam(L6470_KVAL_ACC), + " KVAL_DEC: ", motor.GetParam(L6470_KVAL_DEC), + " V motor max = "); + switch (motor_status) { + case 0: SERIAL_ECHO(motor.GetParam(L6470_KVAL_HOLD) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break; + case 1: SERIAL_ECHO(motor.GetParam(L6470_KVAL_RUN) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_RUN)"); break; + case 2: SERIAL_ECHO(motor.GetParam(L6470_KVAL_ACC) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_ACC)"); break; + case 3: SERIAL_ECHO(motor.GetParam(L6470_KVAL_DEC) * 100 / 256); SERIAL_ECHOPGM("% (KVAL_HOLD)"); break; + } + SERIAL_EOL(); + + #if ENABLED(L6470_CHITCHAT) + DEBUG_ECHOPGM("...SLEW RATE: "); + switch (sh.STATUS_AXIS_LAYOUT) { + case L6470_STATUS_LAYOUT: { + switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) { + case 0: { DEBUG_ECHOLNPGM("320V/uS") ; break; } + case 1: { DEBUG_ECHOLNPGM("75V/uS") ; break; } + case 2: { DEBUG_ECHOLNPGM("110V/uS") ; break; } + case 3: { DEBUG_ECHOLNPGM("260V/uS") ; break; } + } + break; + } + case L6480_STATUS_LAYOUT: { + switch (motor.GetParam(L6470_GATECFG1) & CONFIG1_SR ) { + case CONFIG1_SR_220V_us: { DEBUG_ECHOLNPGM("220V/uS") ; break; } + case CONFIG1_SR_400V_us: { DEBUG_ECHOLNPGM("400V/uS") ; break; } + case CONFIG1_SR_520V_us: { DEBUG_ECHOLNPGM("520V/uS") ; break; } + case CONFIG1_SR_980V_us: { DEBUG_ECHOLNPGM("980V/uS") ; break; } + default: { DEBUG_ECHOLNPGM("unknown") ; break; } + } + } + } + #endif + SERIAL_EOL(); + break; + } + + case L6474_STATUS_LAYOUT: { // L6474 + const uint16_t L6470_ADC_out = motor.GetParam(L6470_ADC_OUT) & 0x1F, + L6474_TVAL_val = motor.GetParam(L6474_TVAL) & 0x7F; + + say_axis_status(axis, sh.STATUS_AXIS_RAW); + + SERIAL_ECHOPGM("...OverCurrent Threshold: "); + sprintf_P(temp_buf, PSTR("%2d ("), OverCurrent_Threshold); + SERIAL_ECHO(temp_buf); + SERIAL_ECHO((OverCurrent_Threshold + 1) * motor.OCD_CURRENT_CONSTANT_INV); + SERIAL_ECHOPGM(" mA)"); + SERIAL_ECHOPGM(" TVAL: "); + sprintf_P(temp_buf, PSTR("%2d ("), L6474_TVAL_val); + SERIAL_ECHO(temp_buf); + SERIAL_ECHO((L6474_TVAL_val + 1) * motor.STALL_CURRENT_CONSTANT_INV); + SERIAL_ECHOLNPGM(" mA) Motor Status: NA"); + + const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07); //NOMORE(MicroSteps, 16); + SERIAL_ECHOPGM("...MicroSteps: ", MicroSteps, + " ADC_OUT: ", L6470_ADC_out); + + SERIAL_ECHOLNPGM(" Vs_compensation: NA\n"); + SERIAL_ECHOLNPGM("...KVAL_HOLD: NA" + " KVAL_RUN : NA" + " KVAL_ACC: NA" + " KVAL_DEC: NA" + " V motor max = NA"); + + #if ENABLED(L6470_CHITCHAT) + DEBUG_ECHOPGM("...SLEW RATE: "); + switch ((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT) { + case 0: DEBUG_ECHOLNPGM("320V/uS") ; break; + case 1: DEBUG_ECHOLNPGM("75V/uS") ; break; + case 2: DEBUG_ECHOLNPGM("110V/uS") ; break; + case 3: DEBUG_ECHOLNPGM("260V/uS") ; break; + default: DEBUG_ECHOLNPGM("slew rate: ", (motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT); break; + } + #endif + SERIAL_EOL(); + SERIAL_EOL(); + break; + } + } +} + +/** + * M906: report or set KVAL_HOLD which sets the maximum effective voltage provided by the + * PWMs to the steppers + * + * On L6474 this sets the TVAL register (same address). + * + * I - select which driver(s) to change on multi-driver axis + * (default) all drivers on the axis + * 0 - monitor only the first XYZ... driver + * 1 - monitor only X2, Y2, Z2 + * 2 - monitor only Z3 + * 3 - monitor only Z4 + * Xxxx, Yxxx, Zxxx, Axxx, Bxxx, Cxxx, Exxx - axis to change (optional) + * L6474 - current in mA (4A max) + * All others - 0-255 + * + * Sets KVAL_HOLD which affects the current being driven through the stepper. + * + * L6470 is used in the STEP-CLOCK mode. KVAL_HOLD is the only KVAL_xxx + * that affects the effective voltage seen by the stepper. + */ +void GcodeSuite::M906() { + + L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status + + #define L6470_SET_KVAL_HOLD(Q) (AXIS_IS_L64XX(Q) ? stepper##Q.setTVALCurrent(value) : stepper##Q.SetParam(L6470_KVAL_HOLD, uint8_t(value))) + + DEBUG_ECHOLNPGM("M906"); + + uint8_t report_current = true; + + #if AXIS_IS_L64XX(X2) || AXIS_IS_L64XX(Y2) || AXIS_IS_L64XX(Z2) || AXIS_IS_L64XX(Z3) || AXIS_IS_L64XX(Z4) + const int8_t index = parser.byteval('I', -1); + #else + constexpr int8_t index = -1; + #endif + + LOOP_LOGICAL_AXES(i) if (uint16_t value = parser.intval(AXIS_CHAR(i))) { + + report_current = false; + + if (planner.has_blocks_queued() || planner.cleaning_buffer_counter) { + SERIAL_ECHOLNPGM("Test aborted. Can't set KVAL_HOLD while steppers are moving."); + return; + } + + switch (i) { + #if AXIS_IS_L64XX(X) || AXIS_IS_L64XX(X2) + case X_AXIS: + #if AXIS_IS_L64XX(X) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(X); + #endif + #if AXIS_IS_L64XX(X2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(X2); + #endif + break; + #endif + + #if AXIS_IS_L64XX(Y) || AXIS_IS_L64XX(Y2) + case Y_AXIS: + #if AXIS_IS_L64XX(Y) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(Y); + #endif + #if AXIS_IS_L64XX(Y2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(Y2); + #endif + break; + #endif + + #if AXIS_IS_L64XX(Z) || AXIS_IS_L64XX(Z2) || AXIS_IS_L64XX(Z3) || AXIS_IS_L64XX(Z4) + case Z_AXIS: + #if AXIS_IS_L64XX(Z) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(Z); + #endif + #if AXIS_IS_L64XX(Z2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + if (index < 0 || index == 2) L6470_SET_KVAL_HOLD(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + if (index < 0 || index == 3) L6470_SET_KVAL_HOLD(Z4); + #endif + break; + #endif + + #if AXIS_IS_L64XX(E0) || AXIS_IS_L64XX(E1) || AXIS_IS_L64XX(E2) || AXIS_IS_L64XX(E3) || AXIS_IS_L64XX(E4) || AXIS_IS_L64XX(E5) || AXIS_IS_L64XX(E6) || AXIS_IS_L64XX(E7) + case E_AXIS: { + const int8_t eindex = get_target_e_stepper_from_command(-2); + #if AXIS_IS_L64XX(E0) + if (eindex < 0 || eindex == 0) L6470_SET_KVAL_HOLD(E0); + #endif + #if AXIS_IS_L64XX(E1) + if (eindex < 0 || eindex == 1) L6470_SET_KVAL_HOLD(E1); + #endif + #if AXIS_IS_L64XX(E2) + if (eindex < 0 || eindex == 2) L6470_SET_KVAL_HOLD(E2); + #endif + #if AXIS_IS_L64XX(E3) + if (eindex < 0 || eindex == 3) L6470_SET_KVAL_HOLD(E3); + #endif + #if AXIS_IS_L64XX(E4) + if (eindex < 0 || eindex == 4) L6470_SET_KVAL_HOLD(E4); + #endif + #if AXIS_IS_L64XX(E5) + if (eindex < 0 || eindex == 5) L6470_SET_KVAL_HOLD(E5); + #endif + #if AXIS_IS_L64XX(E6) + if (eindex < 0 || eindex == 6) L6470_SET_KVAL_HOLD(E6); + #endif + #if AXIS_IS_L64XX(E7) + if (eindex < 0 || eindex == 7) L6470_SET_KVAL_HOLD(E7); + #endif + } break; + #endif + } + } + + if (report_current) { + #define L64XX_REPORT_CURRENT(Q) L64XX_report_current(stepper##Q, Q) + + L64xxManager.spi_active = true; // Tell set_directions() a series of SPI transfers is underway + + #if AXIS_IS_L64XX(X) + L64XX_REPORT_CURRENT(X); + #endif + #if AXIS_IS_L64XX(X2) + L64XX_REPORT_CURRENT(X2); + #endif + #if AXIS_IS_L64XX(Y) + L64XX_REPORT_CURRENT(Y); + #endif + #if AXIS_IS_L64XX(Y2) + L64XX_REPORT_CURRENT(Y2); + #endif + #if AXIS_IS_L64XX(Z) + L64XX_REPORT_CURRENT(Z); + #endif + #if AXIS_IS_L64XX(Z2) + L64XX_REPORT_CURRENT(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + L64XX_REPORT_CURRENT(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + L64XX_REPORT_CURRENT(Z4); + #endif + #if AXIS_IS_L64XX(E0) + L64XX_REPORT_CURRENT(E0); + #endif + #if AXIS_IS_L64XX(E1) + L64XX_REPORT_CURRENT(E1); + #endif + #if AXIS_IS_L64XX(E2) + L64XX_REPORT_CURRENT(E2); + #endif + #if AXIS_IS_L64XX(E3) + L64XX_REPORT_CURRENT(E3); + #endif + #if AXIS_IS_L64XX(E4) + L64XX_REPORT_CURRENT(E4); + #endif + #if AXIS_IS_L64XX(E5) + L64XX_REPORT_CURRENT(E5); + #endif + #if AXIS_IS_L64XX(E6) + L64XX_REPORT_CURRENT(E6); + #endif + #if AXIS_IS_L64XX(E7) + L64XX_REPORT_CURRENT(E7); + #endif + + L64xxManager.spi_active = false; // done with all SPI transfers - clear handshake flags + L64xxManager.spi_abort = false; + L64xxManager.pause_monitor(false); + } +} + +#endif // HAS_L64XX diff --git a/src/gcode/feature/L6470/M916-M918.cpp b/src/gcode/feature/L6470/M916-M918.cpp new file mode 100644 index 0000000..9e1f1b9 --- /dev/null +++ b/src/gcode/feature/L6470/M916-M918.cpp @@ -0,0 +1,650 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +// +// NOTE: All tests assume each axis uses matching driver chips. +// + +#include "../../../inc/MarlinConfig.h" + +#if HAS_L64XX + +#include "../../gcode.h" +#include "../../../module/stepper/indirection.h" +#include "../../../module/planner.h" +#include "../../../libs/L64XX/L64XX_Marlin.h" + +#define DEBUG_OUT ENABLED(L6470_CHITCHAT) +#include "../../../core/debug_out.h" + +/** + * M916: increase KVAL_HOLD until get thermal warning + * NOTE - on L6474 it is TVAL that is used + * + * J - select which driver(s) to monitor on multi-driver axis + * 0 - (default) monitor all drivers on the axis or E0 + * 1 - monitor only X, Y, Z, E1 + * 2 - monitor only X2, Y2, Z2, E2 + * 3 - monitor only Z3, E3 + * 4 - monitor only Z4, E4 + * + * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement + * xxx (1-255) is distance moved on either side of current position + * + * F - feedrate + * optional - will use default max feedrate from configuration.h if not specified + * + * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only + * optional - will report current value from driver if not specified + * + * K - value for KVAL_HOLD (0 - 255) (ignored for L6474) + * optional - will report current value from driver if not specified + * + * D - time (in seconds) to run each setting of KVAL_HOLD/TVAL + * optional - defaults to zero (runs each setting once) + */ + +/** + * This routine is also useful for determining the approximate KVAL_HOLD + * where the stepper stops losing steps. The sound will get noticeably quieter + * as it stops losing steps. + */ + +void GcodeSuite::M916() { + + DEBUG_ECHOLNPGM("M916"); + + L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status + + // Variables used by L64xxManager.get_user_input function - some may not be used + char axis_mon[3][3] = { {" "}, {" "}, {" "} }; // list of Axes to be monitored + L64XX_axis_t axis_index[3]; + uint16_t axis_status[3]; + uint8_t driver_count = 1; + float position_max; + float position_min; + float final_feedrate; + uint8_t kval_hold; + uint8_t OCD_TH_val = 0; + uint8_t STALL_TH_val = 0; + uint16_t over_current_threshold; + constexpr uint8_t over_current_flag = false; // M916 doesn't play with the overcurrent thresholds + + #define DRIVER_TYPE_L6474(Q) AXIS_DRIVER_TYPE_##Q(L6474) + + uint8_t j; // general purpose counter + + if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold)) + return; // quit if invalid user input + + DEBUG_ECHOLNPGM("feedrate = ", final_feedrate); + + planner.synchronize(); // wait for all current movement commands to complete + + const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow; + for (j = 0; j < driver_count; j++) + L64xxManager.get_status(axis_index[j]); // clear out any pre-existing error flags + + char temp_axis_string[] = " "; + temp_axis_string[0] = axis_mon[0][0]; // need to have a string for use within sprintf format section + char gcode_string[80]; + uint16_t status_composite = 0; + + uint16_t M91x_counter = kval_hold; + uint16_t M91x_counter_max; + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { + M91x_counter_max = 128; // TVAL is 7 bits + LIMIT(M91x_counter, 0U, 127U); + } + else + M91x_counter_max = 256; // KVAL_HOLD is 8 bits + + uint8_t M91x_delay_s = parser.byteval('D'); // get delay in seconds + millis_t M91x_delay_ms = SEC_TO_MS(M91x_delay_s * 60); + millis_t M91x_delay_end; + + DEBUG_ECHOLNPGM(".\n."); + + do { + + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) + DEBUG_ECHOLNPGM("TVAL current (mA) = ", (M91x_counter + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV); // report TVAL current for this run + else + DEBUG_ECHOLNPGM("kval_hold = ", M91x_counter); // report KVAL_HOLD for this run + + for (j = 0; j < driver_count; j++) + L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, M91x_counter); //set KVAL_HOLD or TVAL (same register address) + + M91x_delay_end = millis() + M91x_delay_ms; + do { + // turn the motor(s) both directions + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate)); + process_subcommands_now(gcode_string); + + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate)); + process_subcommands_now(gcode_string); + + // get the status after the motors have stopped + planner.synchronize(); + + status_composite = 0; // clear out the old bits + + for (j = 0; j < driver_count; j++) { + axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK; // bits of interest are all active low + status_composite |= axis_status[j] ; + } + + if (status_composite) break; + } while (millis() < M91x_delay_end); + + if (status_composite) break; + + M91x_counter++; + + } while (!(status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) && (M91x_counter < M91x_counter_max)); + + DEBUG_ECHOLNPGM("."); + + #if ENABLED(L6470_CHITCHAT) + if (status_composite) { + L64xxManager.error_status_decode(status_composite, axis_index[0], + sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN, + sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B, + sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT); + DEBUG_ECHOLNPGM("."); + } + #endif + + if ((status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD))) + DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Thermal warning/shutdown has occurred"); + else if (status_composite) + DEBUG_ECHOLNPGM(".\n.\nTest completed abnormally - non-thermal error has occurred"); + else + DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Unable to get to thermal warning/shutdown"); + + L64xxManager.pause_monitor(false); +} + +/** + * M917: Find minimum current thresholds + * + * Decrease OCD current until overcurrent error + * Increase OCD until overcurrent error goes away + * Decrease stall threshold until stall (not done on L6474) + * Increase stall until stall error goes away (not done on L6474) + * + * J - select which driver(s) to monitor on multi-driver axis + * 0 - (default) monitor all drivers on the axis or E0 + * 1 - monitor only X, Y, Z, E1 + * 2 - monitor only X2, Y2, Z2, E2 + * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement + * xxx (1-255) is distance moved on either side of current position + * + * F - feedrate + * optional - will use default max feedrate from Configuration.h if not specified + * + * I - starting over-current threshold + * optional - will report current value from driver if not specified + * if there are multiple drivers on the axis then all will be set the same + * + * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only + * optional - will report current value from driver if not specified + * + * K - value for KVAL_HOLD (0 - 255) (ignored for L6474) + * optional - will report current value from driver if not specified + */ +void GcodeSuite::M917() { + + DEBUG_ECHOLNPGM("M917"); + + L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status + + char axis_mon[3][3] = { {" "}, {" "}, {" "} }; // list of Axes to be monitored + L64XX_axis_t axis_index[3]; + uint16_t axis_status[3]; + uint8_t driver_count = 1; + float position_max; + float position_min; + float final_feedrate; + uint8_t kval_hold; + uint8_t OCD_TH_val = 0; + uint8_t STALL_TH_val = 0; + uint16_t over_current_threshold; + constexpr uint8_t over_current_flag = true; + + uint8_t j; // general purpose counter + + if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold)) + return; // quit if invalid user input + + DEBUG_ECHOLNPGM("feedrate = ", final_feedrate); + + planner.synchronize(); // wait for all current movement commands to complete + + const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow; + for (j = 0; j < driver_count; j++) + L64xxManager.get_status(axis_index[j]); // clear error flags + char temp_axis_string[] = " "; + temp_axis_string[0] = axis_mon[0][0]; // need a sprintf format string + char gcode_string[80]; + uint16_t status_composite = 0; + uint8_t test_phase = 0; // 0 - decreasing OCD - exit when OCD warning occurs (ignore STALL) + // 1 - increasing OCD - exit when OCD warning stops (ignore STALL) + // 2 - OCD finalized - decreasing STALL - exit when STALL warning happens + // 3 - OCD finalized - increasing STALL - exit when STALL warning stop + // 4 - all testing completed + DEBUG_ECHOPGM(".\n.\n.\nover_current threshold : ", (OCD_TH_val + 1) * 375); // first status display + DEBUG_ECHOPGM(" (OCD_TH: : ", OCD_TH_val); + if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) { + DEBUG_ECHOPGM(") Stall threshold: ", (STALL_TH_val + 1) * 31.25); + DEBUG_ECHOPGM(" (STALL_TH: ", STALL_TH_val); + } + DEBUG_ECHOLNPGM(")"); + + do { + + if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) DEBUG_ECHOPGM("STALL threshold : ", (STALL_TH_val + 1) * 31.25); + DEBUG_ECHOLNPGM(" OCD threshold : ", (OCD_TH_val + 1) * 375); + + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate)); + process_subcommands_now(gcode_string); + + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate)); + process_subcommands_now(gcode_string); + + planner.synchronize(); + + status_composite = 0; // clear out the old bits + + for (j = 0; j < driver_count; j++) { + axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK; // bits of interest are all active low + status_composite |= axis_status[j]; + } + + if (status_composite && (status_composite & sh.STATUS_AXIS_UVLO)) { + DEBUG_ECHOLNPGM("Test aborted (Undervoltage lockout active)"); + #if ENABLED(L6470_CHITCHAT) + for (j = 0; j < driver_count; j++) { + if (j) DEBUG_ECHOPGM("..."); + L64xxManager.error_status_decode(axis_status[j], axis_index[j], + sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN, + sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B, + sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT); + } + #endif + return; + } + + if (status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)) { + DEBUG_ECHOLNPGM("thermal problem - waiting for chip(s) to cool down "); + uint16_t status_composite_temp = 0; + uint8_t k = 0; + do { + k++; + if (!(k % 4)) { + kval_hold *= 0.95; + DEBUG_EOL(); + DEBUG_ECHOLNPGM("Lowering KVAL_HOLD by about 5% to ", kval_hold); + for (j = 0; j < driver_count; j++) + L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold); + } + DEBUG_ECHOLNPGM("."); + reset_stepper_timeout(); // keep steppers powered + safe_delay(5000); + status_composite_temp = 0; + for (j = 0; j < driver_count; j++) { + axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & sh.L6470_ERROR_MASK; // bits of interest are all active low + status_composite_temp |= axis_status[j]; + } + } + while (status_composite_temp & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD)); + DEBUG_EOL(); + } + if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B | sh.STATUS_AXIS_OCD)) { + switch (test_phase) { + + case 0: { + if (status_composite & sh.STATUS_AXIS_OCD) { + // phase 0 with OCD warning - time to go to next phase + if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) { + OCD_TH_val = sh.AXIS_OCD_TH_MAX; // limit to max + test_phase = 2; // at highest value so skip phase 1 + //DEBUG_ECHOLNPGM("LOGIC E0A OCD at highest - skip to 2"); + DEBUG_ECHOLNPGM("OCD at highest - OCD finalized"); + } + else { + OCD_TH_val++; // normal exit to next phase + test_phase = 1; // setup for first pass of phase 1 + //DEBUG_ECHOLNPGM("LOGIC E0B - inc OCD & go to 1"); + DEBUG_ECHOLNPGM("inc OCD"); + } + } + else { // phase 0 without OCD warning - keep on decrementing if can + if (OCD_TH_val) { + OCD_TH_val--; // try lower value + //DEBUG_ECHOLNPGM("LOGIC E0C - dec OCD"); + DEBUG_ECHOLNPGM("dec OCD"); + } + else { + test_phase = 2; // at lowest value without warning so skip phase 1 + //DEBUG_ECHOLNPGM("LOGIC E0D - OCD at latest - go to 2"); + DEBUG_ECHOLNPGM("OCD finalized"); + } + } + } break; + + case 1: { + if (status_composite & sh.STATUS_AXIS_OCD) { + // phase 1 with OCD warning - increment if can + if (OCD_TH_val >= sh.AXIS_OCD_TH_MAX) { + OCD_TH_val = sh.AXIS_OCD_TH_MAX; // limit to max + test_phase = 2; // at highest value so go to next phase + //DEBUG_ECHOLNPGM("LOGIC E1A - OCD at max - go to 2"); + DEBUG_ECHOLNPGM("OCD finalized"); + } + else { + OCD_TH_val++; // try a higher value + //DEBUG_ECHOLNPGM("LOGIC E1B - inc OCD"); + DEBUG_ECHOLNPGM("inc OCD"); + } + } + else { // phase 1 without OCD warning - normal exit to phase 2 + test_phase = 2; + //DEBUG_ECHOLNPGM("LOGIC E1C - no OCD warning - go to 1"); + DEBUG_ECHOLNPGM("OCD finalized"); + } + } break; + + case 2: { + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // skip all STALL_TH steps if L6474 + test_phase = 4; + break; + } + if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) { + // phase 2 with stall warning - time to go to next phase + if (STALL_TH_val >= 127) { + STALL_TH_val = 127; // limit to max + //DEBUG_ECHOLNPGM("LOGIC E2A - STALL warning, STALL at max, quit"); + DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning"); + test_phase = 4; + } + else { + test_phase = 3; // normal exit to next phase (found failing value of STALL) + STALL_TH_val++; // setup for first pass of phase 3 + //DEBUG_ECHOLNPGM("LOGIC E2B - INC - STALL warning, inc Stall, go to 3"); + DEBUG_ECHOLNPGM("inc Stall"); + } + } + else { // phase 2 without stall warning - decrement if can + if (STALL_TH_val) { + STALL_TH_val--; // try a lower value + //DEBUG_ECHOLNPGM("LOGIC E2C - no STALL, dec STALL"); + DEBUG_ECHOLNPGM("dec STALL"); + } + else { + DEBUG_ECHOLNPGM("finished - STALL at lowest value but still do NOT have stall warning"); + test_phase = 4; + //DEBUG_ECHOLNPGM("LOGIC E2D - no STALL, at lowest so quit"); + } + } + } break; + + case 3: { + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // skip all STALL_TH steps if L6474 + test_phase = 4; + break; + } + if (status_composite & (sh.STATUS_AXIS_STEP_LOSS_A | sh.STATUS_AXIS_STEP_LOSS_B)) { + // phase 3 with stall warning - increment if can + if (STALL_TH_val >= 127) { + STALL_TH_val = 127; // limit to max + DEBUG_ECHOLNPGM("finished - STALL at maximum value but still have stall warning"); + test_phase = 4; + //DEBUG_ECHOLNPGM("LOGIC E3A - STALL, at max so quit"); + } + else { + STALL_TH_val++; // still looking for passing value + //DEBUG_ECHOLNPGM("LOGIC E3B - STALL, inc stall"); + DEBUG_ECHOLNPGM("inc stall"); + } + } + else { //phase 3 without stall warning but have OCD warning + DEBUG_ECHOLNPGM("Hardware problem - OCD warning without STALL warning"); + test_phase = 4; + //DEBUG_ECHOLNPGM("LOGIC E3C - not STALLED, hardware problem (quit)"); + } + } break; + + } + + } + else { + switch (test_phase) { + case 0: { // phase 0 without OCD warning - keep on decrementing if can + if (OCD_TH_val) { + OCD_TH_val--; // try lower value + //DEBUG_ECHOLNPGM("LOGIC N0A - DEC OCD"); + DEBUG_ECHOLNPGM("DEC OCD"); + } + else { + test_phase = 2; // at lowest value without warning so skip phase 1 + //DEBUG_ECHOLNPGM("LOGIC N0B - OCD at lowest (go to phase 2)"); + DEBUG_ECHOLNPGM("OCD finalized"); + } + } break; + + case 1: //DEBUG_ECHOLNPGM("LOGIC N1 (go directly to 2)"); // phase 1 without OCD warning - drop directly to phase 2 + DEBUG_ECHOLNPGM("OCD finalized"); + + case 2: { // phase 2 without stall warning - keep on decrementing if can + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // skip all STALL_TH steps if L6474 + test_phase = 4; + break; + } + if (STALL_TH_val) { + STALL_TH_val--; // try a lower value (stay in phase 2) + //DEBUG_ECHOLNPGM("LOGIC N2B - dec STALL"); + DEBUG_ECHOLNPGM("dec STALL"); + } + else { + DEBUG_ECHOLNPGM("finished - STALL at lowest value but still no stall warning"); + test_phase = 4; + //DEBUG_ECHOLNPGM("LOGIC N2C - STALL at lowest (quit)"); + } + } break; + + case 3: { + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) { // skip all STALL_TH steps if L6474 + test_phase = 4; + break; + } + test_phase = 4; + //DEBUG_ECHOLNPGM("LOGIC N3 - finished!"); + DEBUG_ECHOLNPGM("finished!"); + } break; // phase 3 without any warnings - desired exit + } // + } // end of status checks + + if (test_phase != 4) { + for (j = 0; j < driver_count; j++) { // update threshold(s) + L64xxManager.set_param(axis_index[j], L6470_OCD_TH, OCD_TH_val); + if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) L64xxManager.set_param(axis_index[j], L6470_STALL_TH, STALL_TH_val); + if (L64xxManager.get_param(axis_index[j], L6470_OCD_TH) != OCD_TH_val) DEBUG_ECHOLNPGM("OCD mismatch"); + if ((L64xxManager.get_param(axis_index[j], L6470_STALL_TH) != STALL_TH_val) && (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT)) DEBUG_ECHOLNPGM("STALL mismatch"); + } + } + + } while (test_phase != 4); + + DEBUG_ECHOLNPGM("."); + if (status_composite) { + #if ENABLED(L6470_CHITCHAT) + for (j = 0; j < driver_count; j++) { + if (j) DEBUG_ECHOPGM("..."); + L64xxManager.error_status_decode(axis_status[j], axis_index[j], + sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN, + sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B, + sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT); + } + DEBUG_ECHOLNPGM("."); + #endif + DEBUG_ECHOLNPGM("Completed with errors"); + } + else + DEBUG_ECHOLNPGM("Completed with no errors"); + DEBUG_ECHOLNPGM("."); + + L64xxManager.pause_monitor(false); +} + +/** + * M918: increase speed until error or max feedrate achieved (as shown in configuration.h)) + * + * J - select which driver(s) to monitor on multi-driver axis + * 0 - (default) monitor all drivers on the axis or E0 + * 1 - monitor only X, Y, Z, E1 + * 2 - monitor only X2, Y2, Z2, E2 + * Xxxx, Yxxx, Zxxx, Exxx - axis to be monitored with displacement + * xxx (1-255) is distance moved on either side of current position + * + * I - over current threshold + * optional - will report current value from driver if not specified + * + * T - current (mA) setting for TVAL (0 - 4A in 31.25mA increments, rounds down) - L6474 only + * optional - will report current value from driver if not specified + * + * K - value for KVAL_HOLD (0 - 255) (ignored for L6474) + * optional - will report current value from driver if not specified + * + * M - value for microsteps (1 - 128) (optional) + * optional - will report current value from driver if not specified + */ +void GcodeSuite::M918() { + + DEBUG_ECHOLNPGM("M918"); + + L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status + + char axis_mon[3][3] = { {" "}, {" "}, {" "} }; // list of Axes to be monitored + L64XX_axis_t axis_index[3]; + uint16_t axis_status[3]; + uint8_t driver_count = 1; + float position_max, position_min; + float final_feedrate; + uint8_t kval_hold; + uint8_t OCD_TH_val = 0; + uint8_t STALL_TH_val = 0; + uint16_t over_current_threshold; + constexpr uint8_t over_current_flag = true; + + const L64XX_Marlin::L64XX_shadow_t &sh = L64xxManager.shadow; + + uint8_t j; // general purpose counter + + if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold)) + return; // quit if invalid user input + + L64xxManager.get_status(axis_index[0]); // populate shadow array + + uint8_t m_steps = parser.byteval('M'); + + if (m_steps != 0) { + LIMIT(m_steps, 1, sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT ? 16 : 128); // L6474 + + uint8_t stepVal; + for (stepVal = 0; stepVal < 8; stepVal++) { // convert to L64xx register value + if (m_steps == 1) break; + m_steps >>= 1; + } + + if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) + stepVal |= 0x98; // NO SYNC + else + stepVal |= (!SYNC_EN) | SYNC_SEL_1 | stepVal; + + for (j = 0; j < driver_count; j++) { + L64xxManager.set_param(axis_index[j], dSPIN_HARD_HIZ, 0); // can't write STEP register if stepper being powered + // results in an extra NOOP being sent (data 00) + L64xxManager.set_param(axis_index[j], L6470_STEP_MODE, stepVal); // set microsteps + } + } + m_steps = L64xxManager.get_param(axis_index[0], L6470_STEP_MODE) & 0x07; // get microsteps + + DEBUG_ECHOLNPGM("Microsteps = ", _BV(m_steps)); + DEBUG_ECHOLNPGM("target (maximum) feedrate = ", final_feedrate); + + const float feedrate_inc = final_feedrate / 10, // Start at 1/10 of max & go up by 1/10 per step + fr_limit = final_feedrate * 0.99f; // Rounding-safe comparison value + float current_feedrate = 0; + + planner.synchronize(); // Wait for moves to complete + + for (j = 0; j < driver_count; j++) + L64xxManager.get_status(axis_index[j]); // Clear error flags + + char temp_axis_string[2] = " "; + temp_axis_string[0] = axis_mon[0][0]; // Need a sprintf format string + //temp_axis_string[1] = '\n'; + + char gcode_string[80]; + uint16_t status_composite = 0; + DEBUG_ECHOLNPGM(".\n.\n."); // Make feedrate outputs easier to read + + do { + current_feedrate += feedrate_inc; + DEBUG_ECHOLNPGM("...feedrate = ", current_feedrate); + + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(current_feedrate)); + process_subcommands_now(gcode_string); + + sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(current_feedrate)); + process_subcommands_now(gcode_string); + + planner.synchronize(); + + for (j = 0; j < driver_count; j++) { + axis_status[j] = (~L64xxManager.get_status(axis_index[j])) & 0x0800; // Bits of interest are all active LOW + status_composite |= axis_status[j]; + } + if (status_composite) break; // Break on any error + } while (current_feedrate < fr_limit); + + DEBUG_ECHOPGM("Completed with "); + if (status_composite) { + DEBUG_ECHOLNPGM("errors"); + #if ENABLED(L6470_CHITCHAT) + for (j = 0; j < driver_count; j++) { + if (j) DEBUG_ECHOPGM("..."); + L64xxManager.error_status_decode(axis_status[j], axis_index[j], + sh.STATUS_AXIS_TH_SD, sh.STATUS_AXIS_TH_WRN, + sh.STATUS_AXIS_STEP_LOSS_A, sh.STATUS_AXIS_STEP_LOSS_B, + sh.STATUS_AXIS_OCD, sh.STATUS_AXIS_LAYOUT); + } + #endif + } + else + DEBUG_ECHOLNPGM("no errors"); + + L64xxManager.pause_monitor(false); +} + +#endif // HAS_L64XX diff --git a/src/gcode/feature/adc/M3426.cpp b/src/gcode/feature/adc/M3426.cpp new file mode 100644 index 0000000..2820c8b --- /dev/null +++ b/src/gcode/feature/adc/M3426.cpp @@ -0,0 +1,68 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(HAS_MCP3426_ADC) + +#include "../../gcode.h" + +#include "../../../feature/adc/adc_mcp3426.h" + +#define MCP3426_BASE_ADDR (0b1101 << 3) + +/** + * M3426: Read 16 bit (signed) value from I2C MCP3426 ADC device + * + * M3426 C channel 1 or 2 + * M3426 G gain 1, 2, 4 or 8 + * M3426 I 0 or 1, invert reply + */ +void GcodeSuite::M3426() { + uint8_t channel = parser.byteval('C', 1), // Channel 1 or 2 + gain = parser.byteval('G', 1), // Gain 1, 2, 4, or 8 + address = parser.byteval('A', 3); // Address 0-7 (or 104-111) + const bool inverted = parser.boolval('I'); + + if (address <= 7) address += MCP3426_BASE_ADDR; + + if (WITHIN(channel, 1, 2) && (gain == 1 || gain == 2 || gain == 4 || gain == 8) && WITHIN(address, MCP3426_BASE_ADDR, MCP3426_BASE_ADDR + 7)) { + int16_t result = mcp3426.ReadValue(channel, gain, address); + + if (mcp3426.Error == false) { + if (inverted) { + // Should we invert the reading (32767 - ADC value) ? + // Caters to end devices that expect values to increase when in reality they decrease. + // e.g., A pressure sensor in a vacuum when the end device expects a positive pressure. + result = INT16_MAX - result; + } + //SERIAL_ECHOPGM(STR_OK); + SERIAL_ECHOLNPGM("V:", result, " C:", channel, " G:", gain, " I:", inverted ? 1 : 0); + } + else + SERIAL_ERROR_MSG("MCP342X i2c error"); + } + else + SERIAL_ERROR_MSG("MCP342X Bad request"); +} + +#endif // HAS_MCP3426_ADC diff --git a/src/gcode/feature/advance/M900.cpp b/src/gcode/feature/advance/M900.cpp new file mode 100644 index 0000000..db09faa --- /dev/null +++ b/src/gcode/feature/advance/M900.cpp @@ -0,0 +1,159 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(LIN_ADVANCE) + +#include "../../gcode.h" +#include "../../../module/planner.h" + +#if ENABLED(EXTRA_LIN_ADVANCE_K) + float other_extruder_advance_K[EXTRUDERS]; + uint8_t lin_adv_slot = 0; +#endif + +/** + * M900: Get or Set Linear Advance K-factor + * T Which tool to address + * K Set current advance K factor (Slot 0). + * L Set secondary advance K factor (Slot 1). Requires EXTRA_LIN_ADVANCE_K. + * S<0/1> Activate slot 0 or 1. Requires EXTRA_LIN_ADVANCE_K. + */ +void GcodeSuite::M900() { + + auto echo_value_oor = [](const char ltr, const bool ten=true) { + SERIAL_CHAR('?', ltr); + SERIAL_ECHOPGM(" value out of range"); + if (ten) SERIAL_ECHOPGM(" (0-10)"); + SERIAL_ECHOLNPGM("."); + }; + + #if EXTRUDERS < 2 + constexpr uint8_t tool_index = 0; + #else + const uint8_t tool_index = parser.intval('T', active_extruder); + if (tool_index >= EXTRUDERS) { + echo_value_oor('T', false); + return; + } + #endif + + float &kref = planner.extruder_advance_K[tool_index], newK = kref; + const float oldK = newK; + + #if ENABLED(EXTRA_LIN_ADVANCE_K) + + float &lref = other_extruder_advance_K[tool_index]; + + const bool old_slot = TEST(lin_adv_slot, tool_index), // The tool's current slot (0 or 1) + new_slot = parser.boolval('S', old_slot); // The passed slot (default = current) + + // If a new slot is being selected swap the current and + // saved K values. Do here so K/L will apply correctly. + if (new_slot != old_slot) { // Not the same slot? + SET_BIT_TO(lin_adv_slot, tool_index, new_slot); // Update the slot for the tool + newK = lref; // Get new K value from backup + lref = oldK; // Save K to backup + } + + // Set the main K value. Apply if the main slot is active. + if (parser.seenval('K')) { + const float K = parser.value_float(); + if (!WITHIN(K, 0, 10)) echo_value_oor('K'); + else if (new_slot) lref = K; // S1 Knn + else newK = K; // S0 Knn + } + + // Set the extra K value. Apply if the extra slot is active. + if (parser.seenval('L')) { + const float L = parser.value_float(); + if (!WITHIN(L, 0, 10)) echo_value_oor('L'); + else if (!new_slot) lref = L; // S0 Lnn + else newK = L; // S1 Lnn + } + + #else + + if (parser.seenval('K')) { + const float K = parser.value_float(); + if (WITHIN(K, 0, 10)) + newK = K; + else + echo_value_oor('K'); + } + + #endif + + if (newK != oldK) { + planner.synchronize(); + kref = newK; + } + + if (!parser.seen_any()) { + + #if ENABLED(EXTRA_LIN_ADVANCE_K) + + #if EXTRUDERS < 2 + SERIAL_ECHOLNPGM("Advance S", new_slot, " K", kref, "(S", !new_slot, " K", lref, ")"); + #else + EXTRUDER_LOOP() { + const bool slot = TEST(lin_adv_slot, e); + SERIAL_ECHOLNPGM("Advance T", e, " S", slot, " K", planner.extruder_advance_K[e], + "(S", !slot, " K", other_extruder_advance_K[e], ")"); + SERIAL_EOL(); + } + #endif + + #else + + SERIAL_ECHO_START(); + #if EXTRUDERS < 2 + SERIAL_ECHOLNPGM("Advance K=", planner.extruder_advance_K[0]); + #else + SERIAL_ECHOPGM("Advance K"); + EXTRUDER_LOOP() { + SERIAL_CHAR(' ', '0' + e, ':'); + SERIAL_DECIMAL(planner.extruder_advance_K[e]); + } + SERIAL_EOL(); + #endif + + #endif + } + +} + +void GcodeSuite::M900_report(const bool forReplay/*=true*/) { + report_heading(forReplay, F(STR_LINEAR_ADVANCE)); + #if EXTRUDERS < 2 + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M900 K", planner.extruder_advance_K[0]); + #else + EXTRUDER_LOOP() { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M900 T", e, " K", planner.extruder_advance_K[e]); + } + #endif +} + +#endif // LIN_ADVANCE diff --git a/src/gcode/feature/baricuda/M126-M129.cpp b/src/gcode/feature/baricuda/M126-M129.cpp new file mode 100644 index 0000000..edeba0d --- /dev/null +++ b/src/gcode/feature/baricuda/M126-M129.cpp @@ -0,0 +1,58 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(BARICUDA) + +#include "../../gcode.h" +#include "../../../feature/baricuda.h" + +#if HAS_HEATER_1 + + /** + * M126: Heater 1 valve open + */ + void GcodeSuite::M126() { baricuda_valve_pressure = parser.byteval('S', 255); } + + /** + * M127: Heater 1 valve close + */ + void GcodeSuite::M127() { baricuda_valve_pressure = 0; } + +#endif // HAS_HEATER_1 + +#if HAS_HEATER_2 + + /** + * M128: Heater 2 valve open + */ + void GcodeSuite::M128() { baricuda_e_to_p_pressure = parser.byteval('S', 255); } + + /** + * M129: Heater 2 valve close + */ + void GcodeSuite::M129() { baricuda_e_to_p_pressure = 0; } + +#endif // HAS_HEATER_2 + +#endif // BARICUDA diff --git a/src/gcode/feature/camera/M240.cpp b/src/gcode/feature/camera/M240.cpp new file mode 100644 index 0000000..19051ff --- /dev/null +++ b/src/gcode/feature/camera/M240.cpp @@ -0,0 +1,195 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(PHOTO_GCODE) + +#include "../../gcode.h" +#include "../../../module/motion.h" // for active_extruder and current_position + +#if PIN_EXISTS(CHDK) + millis_t chdk_timeout; // = 0 +#endif + +#if defined(PHOTO_POSITION) && PHOTO_DELAY_MS > 0 + #include "../../../MarlinCore.h" // for idle() +#endif + +#ifdef PHOTO_RETRACT_MM + + #define _PHOTO_RETRACT_MM (PHOTO_RETRACT_MM + 0) + + #include "../../../module/planner.h" + #include "../../../module/temperature.h" + + #if ENABLED(ADVANCED_PAUSE_FEATURE) + #include "../../../feature/pause.h" + #endif + + #ifdef PHOTO_RETRACT_MM + inline void e_move_m240(const float length, const_feedRate_t fr_mm_s) { + if (length && thermalManager.hotEnoughToExtrude(active_extruder)) + unscaled_e_move(length, fr_mm_s); + } + #endif + +#endif + +#if PIN_EXISTS(PHOTOGRAPH) + + FORCE_INLINE void set_photo_pin(const uint8_t state) { + constexpr uint32_t pulse_length = ( + #ifdef PHOTO_PULSES_US + PHOTO_PULSE_DELAY_US + #else + 15 // 15.24 from _delay_ms(0.01524) + #endif + ); + WRITE(PHOTOGRAPH_PIN, state); + delayMicroseconds(pulse_length); + } + + FORCE_INLINE void tweak_photo_pin() { set_photo_pin(HIGH); set_photo_pin(LOW); } + + #ifdef PHOTO_PULSES_US + + inline void pulse_photo_pin(const uint32_t duration, const uint8_t state) { + if (state) { + for (const uint32_t stop = micros() + duration; micros() < stop;) + tweak_photo_pin(); + } + else + delayMicroseconds(duration); + } + + inline void spin_photo_pin() { + static constexpr uint32_t sequence[] = PHOTO_PULSES_US; + LOOP_L_N(i, COUNT(sequence)) + pulse_photo_pin(sequence[i], !(i & 1)); + } + + #else + + constexpr uint8_t NUM_PULSES = 16; + inline void spin_photo_pin() { for (uint8_t i = NUM_PULSES; i--;) tweak_photo_pin(); } + + #endif +#endif + +/** + * M240: Trigger a camera by... + * + * - CHDK : Emulate a Canon RC-1 with a configurable ON duration. + * https://captain-slow.dk/2014/03/09/3d-printing-timelapses/ + * - PHOTOGRAPH_PIN : Pulse a digital pin 16 times. + * See https://www.doc-diy.net/photo/rc-1_hacked/ + * - PHOTO_SWITCH_POSITION : Bump a physical switch with the X-carriage using a + * configured position, delay, and retract length. + * + * PHOTO_POSITION parameters: + * A - X offset to the return position + * B - Y offset to the return position + * F - Override the XY movement feedrate + * R - Retract/recover length (current units) + * S - Retract/recover feedrate (mm/m) + * X - Move to X before triggering the shutter + * Y - Move to Y before triggering the shutter + * Z - Raise Z by a distance before triggering the shutter + * + * PHOTO_SWITCH_POSITION parameters: + * D - Duration (ms) to hold down switch (Requires PHOTO_SWITCH_MS) + * P - Delay (ms) after triggering the shutter (Requires PHOTO_SWITCH_MS) + * I - Switch trigger position override X + * J - Switch trigger position override Y + */ +void GcodeSuite::M240() { + + #ifdef PHOTO_POSITION + + if (homing_needed_error()) return; + + const xyz_pos_t old_pos = { + current_position.x + parser.linearval('A'), + current_position.y + parser.linearval('B'), + current_position.z + }; + + #ifdef PHOTO_RETRACT_MM + const float rval = parser.linearval('R', _PHOTO_RETRACT_MM); + const feedRate_t sval = parser.feedrateval('S', TERN(ADVANCED_PAUSE_FEATURE, PAUSE_PARK_RETRACT_FEEDRATE, TERN(FWRETRACT, RETRACT_FEEDRATE, 45))); + e_move_m240(-rval, sval); + #endif + + feedRate_t fr_mm_s = MMM_TO_MMS(parser.linearval('F')); + if (fr_mm_s) NOLESS(fr_mm_s, 10.0f); + + constexpr xyz_pos_t photo_position = PHOTO_POSITION; + xyz_pos_t raw = { + parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : photo_position.x, + parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_position.y, + (parser.seenval('Z') ? parser.value_linear_units() : photo_position.z) + current_position.z + }; + apply_motion_limits(raw); + do_blocking_move_to(raw, fr_mm_s); + + #ifdef PHOTO_SWITCH_POSITION + constexpr xy_pos_t photo_switch_position = PHOTO_SWITCH_POSITION; + const xy_pos_t sraw = { + parser.seenval('I') ? RAW_X_POSITION(parser.value_linear_units()) : photo_switch_position.x, + parser.seenval('J') ? RAW_Y_POSITION(parser.value_linear_units()) : photo_switch_position.y + }; + do_blocking_move_to_xy(sraw, get_homing_bump_feedrate(X_AXIS)); + #if PHOTO_SWITCH_MS > 0 + safe_delay(parser.intval('D', PHOTO_SWITCH_MS)); + #endif + do_blocking_move_to(raw); + #endif + + #endif + + #if PIN_EXISTS(CHDK) + + OUT_WRITE(CHDK_PIN, HIGH); + chdk_timeout = millis() + parser.intval('D', PHOTO_SWITCH_MS); + + #elif HAS_PHOTOGRAPH + + spin_photo_pin(); + delay(7.33); + spin_photo_pin(); + + #endif + + #ifdef PHOTO_POSITION + #if PHOTO_DELAY_MS > 0 + const millis_t timeout = millis() + parser.intval('P', PHOTO_DELAY_MS); + while (PENDING(millis(), timeout)) idle(); + #endif + do_blocking_move_to(old_pos, fr_mm_s); + #ifdef PHOTO_RETRACT_MM + e_move_m240(rval, sval); + #endif + #endif +} + +#endif // PHOTO_GCODE diff --git a/src/gcode/feature/cancel/M486.cpp b/src/gcode/feature/cancel/M486.cpp new file mode 100644 index 0000000..c1e90d1 --- /dev/null +++ b/src/gcode/feature/cancel/M486.cpp @@ -0,0 +1,57 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(CANCEL_OBJECTS) + +#include "../../gcode.h" +#include "../../../feature/cancel_object.h" + +/** + * M486: A simple interface to cancel objects + * + * T[count] : Reset objects and/or set the count + * S : Start an object with the given index + * P : Cancel the object with the given index + * U : Un-cancel object with the given index + * C : Cancel the current object (the last index given by S) + * S-1 : Start a non-object like a brim or purge tower that should always print + */ +void GcodeSuite::M486() { + + if (parser.seen('T')) { + cancelable.reset(); + cancelable.object_count = parser.intval('T', 1); + } + + if (parser.seenval('S')) + cancelable.set_active_object(parser.value_int()); + + if (parser.seen('C')) cancelable.cancel_active_object(); + + if (parser.seenval('P')) cancelable.cancel_object(parser.value_int()); + + if (parser.seenval('U')) cancelable.uncancel_object(parser.value_int()); +} + +#endif // CANCEL_OBJECTS diff --git a/src/gcode/feature/caselight/M355.cpp b/src/gcode/feature/caselight/M355.cpp new file mode 100644 index 0000000..b3b863f --- /dev/null +++ b/src/gcode/feature/caselight/M355.cpp @@ -0,0 +1,73 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(CASE_LIGHT_ENABLE) + +#include "../../../feature/caselight.h" +#include "../../gcode.h" + +/** + * M355: Turn case light on/off and set brightness + * + * P Set case light brightness (PWM pin required - ignored otherwise) + * + * S Set case light on/off + * + * When S turns on the light on a PWM pin then the current brightness level is used/restored + * + * M355 P200 S0 turns off the light & sets the brightness level + * M355 S1 turns on the light with a brightness of 200 (assuming a PWM pin) + */ +void GcodeSuite::M355() { + bool didset = false; + #if CASELIGHT_USES_BRIGHTNESS + if (parser.seenval('P')) { + didset = true; + caselight.brightness = parser.value_byte(); + } + #endif + const bool sflag = parser.seenval('S'); + if (sflag) { + didset = true; + caselight.on = parser.value_bool(); + } + if (didset) caselight.update(sflag); + + // Always report case light status + SERIAL_ECHO_START(); + SERIAL_ECHOPGM("Case light: "); + if (!caselight.on) + SERIAL_ECHOLNPGM(STR_OFF); + else { + #if CASELIGHT_USES_BRIGHTNESS + if (TERN(CASE_LIGHT_USE_NEOPIXEL, true, TERN0(NEED_CASE_LIGHT_PIN, PWM_PIN(CASE_LIGHT_PIN)))) { + SERIAL_ECHOLN(int(caselight.brightness)); + return; + } + #endif + SERIAL_ECHOLNPGM(STR_ON); + } +} + +#endif // CASE_LIGHT_ENABLE diff --git a/src/gcode/feature/clean/G12.cpp b/src/gcode/feature/clean/G12.cpp new file mode 100644 index 0000000..0113170 --- /dev/null +++ b/src/gcode/feature/clean/G12.cpp @@ -0,0 +1,83 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(NOZZLE_CLEAN_FEATURE) + +#include "../../../libs/nozzle.h" + +#include "../../gcode.h" +#include "../../parser.h" +#include "../../../module/motion.h" + +#if HAS_LEVELING + #include "../../../module/planner.h" + #include "../../../feature/bedlevel/bedlevel.h" +#endif + +/** + * G12: Clean the nozzle + * + * E : 0=Never or 1=Always apply the "software endstop" limits + * P0 S : Stroke cleaning with S strokes + * P1 Sn T : Zigzag cleaning with S repeats and T zigzags + * P2 Sn R : Circle cleaning with S repeats and R radius + * X, Y, Z : Specify axes to move during cleaning. Default: ALL. + */ +void GcodeSuite::G12() { + + // Don't allow nozzle cleaning without homing first + constexpr main_axes_bits_t clean_axis_mask = main_axes_mask & ~TERN0(NOZZLE_CLEAN_NO_Z, Z_AXIS) & ~TERN0(NOZZLE_CLEAN_NO_Y, Y_AXIS); + if (homing_needed_error(clean_axis_mask)) return; + + #ifdef WIPE_SEQUENCE_COMMANDS + if (!parser.seen_any()) { + process_subcommands_now(F(WIPE_SEQUENCE_COMMANDS)); + return; + } + #endif + + const uint8_t pattern = parser.ushortval('P', 0), + strokes = parser.ushortval('S', NOZZLE_CLEAN_STROKES), + objects = parser.ushortval('T', NOZZLE_CLEAN_TRIANGLES); + const float radius = parser.linearval('R', NOZZLE_CLEAN_CIRCLE_RADIUS); + + const bool seenxyz = parser.seen("XYZ"); + const uint8_t cleans = (!seenxyz || parser.boolval('X') ? _BV(X_AXIS) : 0) + | (!seenxyz || parser.boolval('Y') ? _BV(Y_AXIS) : 0) + | TERN(NOZZLE_CLEAN_NO_Z, 0, (!seenxyz || parser.boolval('Z') ? _BV(Z_AXIS) : 0)) + ; + + #if HAS_LEVELING + // Disable bed leveling if cleaning Z + TEMPORARY_BED_LEVELING_STATE(!TEST(cleans, Z_AXIS) && planner.leveling_active); + #endif + + SET_SOFT_ENDSTOP_LOOSE(!parser.boolval('E')); + + nozzle.clean(pattern, strokes, radius, objects, cleans); + + SET_SOFT_ENDSTOP_LOOSE(false); +} + +#endif // NOZZLE_CLEAN_FEATURE diff --git a/src/gcode/feature/controllerfan/M710.cpp b/src/gcode/feature/controllerfan/M710.cpp new file mode 100644 index 0000000..b98d888 --- /dev/null +++ b/src/gcode/feature/controllerfan/M710.cpp @@ -0,0 +1,81 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfigPre.h" + +#if ENABLED(CONTROLLER_FAN_EDITABLE) + +#include "../../gcode.h" +#include "../../../feature/controllerfan.h" + +/** + * M710: Set controller fan settings + * + * R : Reset to defaults + * S[0-255] : Fan speed when motors are active + * I[0-255] : Fan speed when motors are idle + * A[0|1] : Turn auto mode on or off + * D : Set auto mode idle duration + * + * Examples: + * M710 ; Report current Settings + * M710 R ; Reset SIAD to defaults + * M710 I64 ; Set controller fan Idle Speed to 25% + * M710 S255 ; Set controller fan Active Speed to 100% + * M710 S0 ; Set controller fan Active Speed to OFF + * M710 I255 A0 ; Set controller fan Idle Speed to 100% with Auto Mode OFF + * M710 I127 A1 S255 D160 ; Set controller fan idle speed 50%, AutoMode On, Fan speed 100%, duration to 160 Secs + */ +void GcodeSuite::M710() { + + const bool seenR = parser.seen('R'); + if (seenR) controllerFan.reset(); + + const bool seenS = parser.seenval('S'); + if (seenS) controllerFan.settings.active_speed = parser.value_byte(); + + const bool seenI = parser.seenval('I'); + if (seenI) controllerFan.settings.idle_speed = parser.value_byte(); + + const bool seenA = parser.seenval('A'); + if (seenA) controllerFan.settings.auto_mode = parser.value_bool(); + + const bool seenD = parser.seenval('D'); + if (seenD) controllerFan.settings.duration = parser.value_ushort(); + + if (!(seenR || seenS || seenI || seenA || seenD)) + M710_report(); +} + +void GcodeSuite::M710_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_CONTROLLER_FAN)); + SERIAL_ECHOLNPGM(" M710" + " S", int(controllerFan.settings.active_speed), + " I", int(controllerFan.settings.idle_speed), + " A", int(controllerFan.settings.auto_mode), + " D", controllerFan.settings.duration, + " ; (", (int(controllerFan.settings.active_speed) * 100) / 255, "%" + " ", (int(controllerFan.settings.idle_speed) * 100) / 255, "%)" + ); +} + +#endif // CONTROLLER_FAN_EDITABLE diff --git a/src/gcode/feature/digipot/M907-M910.cpp b/src/gcode/feature/digipot/M907-M910.cpp new file mode 100644 index 0000000..372cb4b --- /dev/null +++ b/src/gcode/feature/digipot/M907-M910.cpp @@ -0,0 +1,173 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM || HAS_MOTOR_CURRENT_I2C || HAS_MOTOR_CURRENT_DAC + +#include "../../gcode.h" + +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + #include "../../../module/stepper.h" +#endif + +#if HAS_MOTOR_CURRENT_I2C + #include "../../../feature/digipot/digipot.h" +#endif + +#if HAS_MOTOR_CURRENT_DAC + #include "../../../feature/dac/stepper_dac.h" +#endif + +/** + * M907: Set digital trimpot motor current using axis codes X [Y] [Z] [I] [J] [K] [E] + * B - Special case for E1 (Requires DIGIPOTSS_PIN or DIGIPOT_MCP4018 or DIGIPOT_MCP4451) + * C - Special case for E2 (Requires DIGIPOTSS_PIN or DIGIPOT_MCP4018 or DIGIPOT_MCP4451) + * S - Set current in mA for all axes (Requires DIGIPOTSS_PIN or DIGIPOT_MCP4018 or DIGIPOT_MCP4451), or + * Set percentage of max current for all axes (Requires HAS_DIGIPOT_DAC) + */ +void GcodeSuite::M907() { + #if HAS_MOTOR_CURRENT_SPI + + if (!parser.seen("BS" STR_AXES_LOGICAL)) + return M907_report(); + + if (parser.seenval('S')) LOOP_L_N(i, MOTOR_CURRENT_COUNT) stepper.set_digipot_current(i, parser.value_int()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(IAXIS_CHAR(i))) stepper.set_digipot_current(i, parser.value_int()); // X Y Z (I J K) E (map to drivers according to DIGIPOT_CHANNELS. Default with NUM_AXES 3: map X Y Z E to X Y Z E0) + // Additional extruders use B,C. + // TODO: Change these parameters because 'E' is used and D should be reserved for debugging. B? + #if E_STEPPERS >= 2 + if (parser.seenval('B')) stepper.set_digipot_current(E_AXIS + 1, parser.value_int()); + #if E_STEPPERS >= 3 + if (parser.seenval('C')) stepper.set_digipot_current(E_AXIS + 2, parser.value_int()); + #endif + #endif + + #elif HAS_MOTOR_CURRENT_PWM + + #if ANY_PIN(MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y, MOTOR_CURRENT_PWM_XY, MOTOR_CURRENT_PWM_I, MOTOR_CURRENT_PWM_J, MOTOR_CURRENT_PWM_K) + #define HAS_X_Y_XY_I_J_K 1 + #endif + + #if HAS_X_Y_XY_I_J_K || ANY_PIN(MOTOR_CURRENT_PWM_E, MOTOR_CURRENT_PWM_Z) + + if (!parser.seen("S" + #if HAS_X_Y_XY_I_J_K + "XY" SECONDARY_AXIS_GANG("I", "J", "K") + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + "Z" + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + "E" + #endif + )) return M907_report(); + + if (parser.seenval('S')) LOOP_L_N(a, MOTOR_CURRENT_COUNT) stepper.set_digipot_current(a, parser.value_int()); + + #if HAS_X_Y_XY_I_J_K + if (NUM_AXIS_GANG( + parser.seenval('X'), || parser.seenval('Y'), || false, + || parser.seenval('I'), || parser.seenval('J'), || parser.seenval('K') + )) stepper.set_digipot_current(0, parser.value_int()); + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + if (parser.seenval('Z')) stepper.set_digipot_current(1, parser.value_int()); + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + if (parser.seenval('E')) stepper.set_digipot_current(2, parser.value_int()); + #endif + + #endif + + #endif // HAS_MOTOR_CURRENT_PWM + + #if HAS_MOTOR_CURRENT_I2C + // this one uses actual amps in floating point + if (parser.seenval('S')) LOOP_L_N(q, DIGIPOT_I2C_NUM_CHANNELS) digipot_i2c.set_current(q, parser.value_float()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(IAXIS_CHAR(i))) digipot_i2c.set_current(i, parser.value_float()); // X Y Z (I J K) E (map to drivers according to pots adresses. Default with NUM_AXES 3 X Y Z E: map to X Y Z E0) + // Additional extruders use B,C,D. + // TODO: Change these parameters because 'E' is used and because 'D' should be reserved for debugging. B? + #if E_STEPPERS >= 2 + for (uint8_t i = E_AXIS + 1; i < _MAX(DIGIPOT_I2C_NUM_CHANNELS, (NUM_AXES + 3)); i++) + if (parser.seenval('B' + i - (E_AXIS + 1))) digipot_i2c.set_current(i, parser.value_float()); + #endif + #endif + + #if HAS_MOTOR_CURRENT_DAC + if (parser.seenval('S')) { + const float dac_percent = parser.value_float(); + LOOP_LOGICAL_AXES(i) stepper_dac.set_current_percent(i, dac_percent); + } + LOOP_LOGICAL_AXES(i) if (parser.seenval(IAXIS_CHAR(i))) stepper_dac.set_current_percent(i, parser.value_float()); // X Y Z (I J K) E (map to drivers according to DAC_STEPPER_ORDER. Default with NUM_AXES 3: X Y Z E map to X Y Z E0) + #endif +} + +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + + void GcodeSuite::M907_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_STEPPER_MOTOR_CURRENTS)); + #if HAS_MOTOR_CURRENT_PWM + SERIAL_ECHOLNPGM_P( // PWM-based has 3 values: + PSTR(" M907 X"), stepper.motor_current_setting[0] // X, Y, (I, J, K) + , SP_Z_STR, stepper.motor_current_setting[1] // Z + , SP_E_STR, stepper.motor_current_setting[2] // E + ); + #elif HAS_MOTOR_CURRENT_SPI + SERIAL_ECHOPGM(" M907"); // SPI-based has 5 values: + LOOP_LOGICAL_AXES(q) { // X Y Z (I J K) E (map to X Y Z (I J K) E0 by default) + SERIAL_CHAR(' ', IAXIS_CHAR(q)); + SERIAL_ECHO(stepper.motor_current_setting[q]); + } + #if E_STEPPERS >= 2 + SERIAL_ECHOPGM_P(PSTR(" B"), stepper.motor_current_setting[E_AXIS + 1] // B (maps to E1 with NUM_AXES 3 according to DIGIPOT_CHANNELS) + #if E_STEPPERS >= 3 + , PSTR(" C"), stepper.motor_current_setting[E_AXIS + 2] // C (mapping to E2 must be defined by DIGIPOT_CHANNELS) + #endif + ); + #endif + SERIAL_EOL(); + #endif + } + +#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_DAC + + /** + * M908: Control digital trimpot directly (M908 P S) + */ + void GcodeSuite::M908() { + TERN_(HAS_MOTOR_CURRENT_SPI, stepper.set_digipot_value_spi(parser.intval('P'), parser.intval('S'))); + TERN_(HAS_MOTOR_CURRENT_DAC, stepper_dac.set_current_value(parser.byteval('P', -1), parser.ushortval('S', 0))); + } + + #if HAS_MOTOR_CURRENT_DAC + + void GcodeSuite::M909() { stepper_dac.print_values(); } + void GcodeSuite::M910() { stepper_dac.commit_eeprom(); } + + #endif // HAS_MOTOR_CURRENT_DAC + +#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_DAC + +#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM || HAS_MOTOR_CURRENT_I2C || HAS_MOTOR_CURRENT_DAC diff --git a/src/gcode/feature/filwidth/M404-M407.cpp b/src/gcode/feature/filwidth/M404-M407.cpp new file mode 100644 index 0000000..ff174ec --- /dev/null +++ b/src/gcode/feature/filwidth/M404-M407.cpp @@ -0,0 +1,71 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FILAMENT_WIDTH_SENSOR) + +#include "../../../feature/filwidth.h" +#include "../../../module/planner.h" +#include "../../../MarlinCore.h" +#include "../../gcode.h" + +/** + * M404: Display or set (in current units) the nominal filament width (3mm, 1.75mm ) W<3.0> + */ +void GcodeSuite::M404() { + if (parser.seenval('W')) { + filwidth.nominal_mm = parser.value_linear_units(); + planner.volumetric_area_nominal = CIRCLE_AREA(filwidth.nominal_mm * 0.5); + } + else + SERIAL_ECHOLNPGM("Filament dia (nominal mm):", filwidth.nominal_mm); +} + +/** + * M405: Turn on filament sensor for control + */ +void GcodeSuite::M405() { + // This is technically a linear measurement, but since it's quantized to centimeters and is a different + // unit than everything else, it uses parser.value_byte() instead of parser.value_linear_units(). + if (parser.seenval('D')) + filwidth.set_delay_cm(parser.value_byte()); + + filwidth.enable(true); +} + +/** + * M406: Turn off filament sensor for control + */ +void GcodeSuite::M406() { + filwidth.enable(false); + planner.calculate_volumetric_multipliers(); // Restore correct 'volumetric_multiplier' value +} + +/** + * M407: Get measured filament diameter on serial output + */ +void GcodeSuite::M407() { + SERIAL_ECHOLNPGM("Filament dia (measured mm):", filwidth.measured_mm); +} + +#endif // FILAMENT_WIDTH_SENSOR diff --git a/src/gcode/feature/fwretract/G10_G11.cpp b/src/gcode/feature/fwretract/G10_G11.cpp new file mode 100644 index 0000000..1889f83 --- /dev/null +++ b/src/gcode/feature/fwretract/G10_G11.cpp @@ -0,0 +1,42 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FWRETRACT) + +#include "../../../feature/fwretract.h" +#include "../../gcode.h" +#include "../../../module/motion.h" + +/** + * G10 - Retract filament according to settings of M207 + * TODO: Handle 'G10 P' for tool settings and 'G10 L' for workspace settings + */ +void GcodeSuite::G10() { fwretract.retract(true E_OPTARG(parser.boolval('S'))); } + +/** + * G11 - Recover filament according to settings of M208 + */ +void GcodeSuite::G11() { fwretract.retract(false); } + +#endif // FWRETRACT diff --git a/src/gcode/feature/fwretract/M207-M209.cpp b/src/gcode/feature/fwretract/M207-M209.cpp new file mode 100644 index 0000000..173c289 --- /dev/null +++ b/src/gcode/feature/fwretract/M207-M209.cpp @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FWRETRACT) + +#include "../../../feature/fwretract.h" +#include "../../gcode.h" + +/** + * M207: Set firmware retraction values + * + * S[+units] retract_length + * W[+units] swap_retract_length (multi-extruder) + * F[units/min] retract_feedrate_mm_s + * Z[units] retract_zraise + */ +void GcodeSuite::M207() { fwretract.M207(); } + +void GcodeSuite::M207_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_RETRACT_S_F_Z)); + fwretract.M207_report(); +} + +/** + * M208: Set firmware un-retraction values + * + * S[+units] retract_recover_extra (in addition to M207 S*) + * W[+units] swap_retract_recover_extra (multi-extruder) + * F[units/min] retract_recover_feedrate_mm_s + * R[units/min] swap_retract_recover_feedrate_mm_s + */ +void GcodeSuite::M208() { fwretract.M208(); } + +void GcodeSuite::M208_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_RECOVER_S_F)); + fwretract.M208_report(); +} + +#if ENABLED(FWRETRACT_AUTORETRACT) + + /** + * M209: Enable automatic retract (M209 S1) + * + * For slicers that don't support G10/11, reversed + * extruder-only moves can be classified as retraction. + */ + void GcodeSuite::M209() { fwretract.M209(); } + + void GcodeSuite::M209_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_AUTO_RETRACT_S)); + fwretract.M209_report(); + } + +#endif + +#endif // FWRETRACT diff --git a/src/gcode/feature/i2c/M260_M261.cpp b/src/gcode/feature/i2c/M260_M261.cpp new file mode 100644 index 0000000..cf9bb7e --- /dev/null +++ b/src/gcode/feature/i2c/M260_M261.cpp @@ -0,0 +1,77 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program 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. + * + * This program 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 this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(EXPERIMENTAL_I2CBUS) + +#include "../../gcode.h" + +#include "../../../feature/twibus.h" + +/** + * M260: Send data to a I2C slave device + * + * This is a PoC, the formatting and arguments for the GCODE will + * change to be more compatible, the current proposal is: + * + * M260 A ; Sets the I2C slave address the data will be sent to + * + * M260 B + * M260 B + * M260 B + * + * M260 S1 ; Send the buffered data and reset the buffer + * M260 R1 ; Reset the buffer without sending data + */ +void GcodeSuite::M260() { + // Set the target address + if (parser.seenval('A')) i2c.address(parser.value_byte()); + + // Add a new byte to the buffer + if (parser.seenval('B')) i2c.addbyte(parser.value_byte()); + + // Flush the buffer to the bus + if (parser.seen('S')) i2c.send(); + + // Reset and rewind the buffer + else if (parser.seen('R')) i2c.reset(); +} + +/** + * M261: Request X bytes from I2C slave device + * + * Usage: M261 A B S