From a693639787234e91258892809d8ffa06cb18209a Mon Sep 17 00:00:00 2001 From: Paciente8159 Date: Thu, 7 Nov 2024 16:36:24 +0000 Subject: [PATCH] initial RP2350 support --- platformio.ini | 1 + uCNC/cnc_config.h | 2 +- .../hal/boards/rp2350/boardmap_rpi_pico2.h | 152 ++ uCNC/src/hal/boards/rp2350/rp2350.ini | 46 + uCNC/src/hal/mcus/mcudefs.h | 7 + uCNC/src/hal/mcus/mcus.h | 1 + uCNC/src/hal/mcus/rp2040/rp2040_arduino.cpp | 2 +- uCNC/src/hal/mcus/rp2350/README.md | 17 + uCNC/src/hal/mcus/rp2350/ic74hc595.pio | 57 + uCNC/src/hal/mcus/rp2350/ic74hc595.pio.h | 71 + uCNC/src/hal/mcus/rp2350/mcu_rp2350.c | 935 +++++++++++ uCNC/src/hal/mcus/rp2350/mcumap_rp2350.h | 1467 +++++++++++++++++ uCNC/src/hal/mcus/rp2350/rp2350_arduino.cpp | 1465 ++++++++++++++++ 13 files changed, 4221 insertions(+), 2 deletions(-) create mode 100644 uCNC/src/hal/boards/rp2350/boardmap_rpi_pico2.h create mode 100644 uCNC/src/hal/boards/rp2350/rp2350.ini create mode 100644 uCNC/src/hal/mcus/rp2350/README.md create mode 100644 uCNC/src/hal/mcus/rp2350/ic74hc595.pio create mode 100644 uCNC/src/hal/mcus/rp2350/ic74hc595.pio.h create mode 100644 uCNC/src/hal/mcus/rp2350/mcu_rp2350.c create mode 100644 uCNC/src/hal/mcus/rp2350/mcumap_rp2350.h create mode 100644 uCNC/src/hal/mcus/rp2350/rp2350_arduino.cpp diff --git a/platformio.ini b/platformio.ini index f0207f972..2c286e40e 100644 --- a/platformio.ini +++ b/platformio.ini @@ -20,6 +20,7 @@ extra_configs = uCNC/src/hal/boards/lpc176x/lpc176x.ini uCNC/src/hal/boards/esp32/esp32.ini uCNC/src/hal/boards/rp2040/rp2040.ini + uCNC/src/hal/boards/rp2350/rp2350.ini uCNC/src/hal/mcus/virtual/virtual.ini [env] diff --git a/uCNC/cnc_config.h b/uCNC/cnc_config.h index 0b52f0fc9..b7a0f5463 100644 --- a/uCNC/cnc_config.h +++ b/uCNC/cnc_config.h @@ -125,7 +125,7 @@ extern "C" * This is useful if you don't have EEPROM/FLASH storage or the divide read/write maximum cycle count is low to prevent damage * This is also usefull if the sender provides all settings at startup/connection * */ - // #define RAM_ONLY_SETTINGS + // #define RAM_ONLY_SETTINGS /** * Override default configuration settings. Use _PER_AXIS parameters to diff --git a/uCNC/src/hal/boards/rp2350/boardmap_rpi_pico2.h b/uCNC/src/hal/boards/rp2350/boardmap_rpi_pico2.h new file mode 100644 index 000000000..572c05132 --- /dev/null +++ b/uCNC/src/hal/boards/rp2350/boardmap_rpi_pico2.h @@ -0,0 +1,152 @@ +/* + Name: boardmap_rp_pico2.h + Description: Contains all MCU and PIN definitions for Raspberry Pi Pico2 to run µCNC. + + Copyright: Copyright (c) João Martins + Author: João Martins + Date: 07-11-2024 + + µCNC is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. Please see + + µCNC is distributed WITHOUT ANY WARRANTY; + Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + See the GNU General Public License for more details. +*/ + +#ifndef BOARDMAP_RPI_PICO2_H +#define BOARDMAP_RPI_PICO2_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#ifndef MCU +#define MCU MCU_RP2350 +#endif + +#ifndef BOARD_NAME +#define BOARD_NAME "RPi Pico2" +#endif + +// SAME AS GRBL for test purposes +// Setup step pins +#define STEP2_BIT 10 // assigns STEP2 pin +#define STEP1_BIT 6 // assigns STEP1 pin +#define STEP0_BIT 2 // assigns STEP0 pin + +// Setup dir pins +#define DIR2_BIT 11 // assigns DIR2 pin +#define DIR1_BIT 7 // assigns DIR1 pin +#define DIR0_BIT 3 // assigns DIR0 pin + +#define LIMIT_Z_BIT 13 +#define LIMIT_Y_BIT 9 +#define LIMIT_X_BIT 5 + +#define LIMIT_Z_PULLUP +#define LIMIT_Y_PULLUP +#define LIMIT_X_PULLUP + +// Setup control input pins +// #define ESTOP_BIT 0 +// #define ESTOP_PORT A +// #define ESTOP_ISR + +// Setup com pins +#define RX_BIT 1 +#define RX_PULLUP +#define TX_BIT 0 +// only uncomment this if other port other then 0 is used +// #define UART_PORT 0 + +// forces USB +#define MCU_HAS_USB + + // Setup PWM +#define PWM0_BIT 14 // assigns PWM0 pin + +// Setup generic IO Pins +// spindle dir +#define DOUT0_BIT 15 + +// Stepper enable pin. For Grbl on Uno board a single pin is used +#define STEP2_EN_BIT 12 +#define STEP1_EN_BIT 8 +#define STEP0_EN_BIT 4 + +// activity LED +#define DOUT31_BIT 25 + + // disable EEPROM emulation + // #ifndef RAM_ONLY_SETTINGS + // #define RAM_ONLY_SETTINGS + // #endif + +#define I2C_CLK_BIT 27 +#define I2C_DATA_BIT 26 +#define I2C_PORT 1 + // #define I2C_ADDRESS 1 + + /** + * This is an example of how to use RP2040 PIO to control + * up to 4 chainned 74hc595 (32 output pins) using only 3 pins + * from the board. + * The 3 pins should be sequential (for example GPIO's 26, 27 and 28) + * + * RP2040 does not yet support software generate PWM + * + * **/ + + // // Use PIO to shift data to 74HC595 shift registers (up to 4) + // // IO pins should be sequencial GPIO pins starting by data, then clock then the latch pin + // #define IC74HC595_CUSTOM_SHIFT_IO //Enables custom MCU data shift transmission. In RP2040 that is via a PIO + // #define IC74HC595_PIO_DATA 26 + // #define IC74HC595_PIO_CLK 27 + // #define IC74HC595_PIO_LATCH 28 + // // enabling IC74HC595_CUSTOM_SHIFT_IO will force IC74HC595_COUNT to be set to 4 no matter what + // // support up to 4 chained 74HC595. Less can be used (overflow bits will be discarded like in the ESP32 I2S implementation) + // #define IC74HC595_COUNT 4 + + // #define STEP0_EN_IO_OFFSET 0 + // #define STEP0_IO_OFFSET 1 + // #define DIR0_IO_OFFSET 2 + // #define STEP1_EN_IO_OFFSET 3 + // #define STEP1_IO_OFFSET 4 + // #define DIR1_IO_OFFSET 5 + // #define STEP2_EN_IO_OFFSET 6 + // #define STEP2_IO_OFFSET 7 + // #define DIR2_IO_OFFSET 8 + // #define STEP3_EN_IO_OFFSET 9 + // #define STEP3_IO_OFFSET 10 + // #define DIR3_IO_OFFSET 11 + // #define STEP4_EN_IO_OFFSET 12 + // #define STEP4_IO_OFFSET 13 + // #define DIR4_IO_OFFSET 14 + // #define DOUT0_IO_OFFSET 15 + +#define SPI2_CLK_BIT 18 +#define SPI2_SDO_BIT 19 +#define SPI2_SDI_BIT 16 +#define SPI2_CS_BIT 17 +#define SPI2_PORT 0 + +// #define SPI2_CLK_BIT 10 +// #define SPI2_SDO_BIT 11 +// #define SPI2_SDI_BIT 12 +// #define SPI2_CS_BIT 13 +// #define SPI2_PORT 1 + + /** + * New multicore mode - Experimental + * */ + // #define RP2040_RUN_MULTICORE + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/uCNC/src/hal/boards/rp2350/rp2350.ini b/uCNC/src/hal/boards/rp2350/rp2350.ini new file mode 100644 index 000000000..98832e4ca --- /dev/null +++ b/uCNC/src/hal/boards/rp2350/rp2350.ini @@ -0,0 +1,46 @@ +################## +# RP2350 Boards # +################## + +[common_rp2350] +platform = https://github.com/maxgerhardt/platform-raspberrypi.git +platform_packages = framework-arduinopico@https://github.com/earlephilhower/arduino-pico.git +framework = arduino +board_build.core = earlephilhower +board_build.filesystem_size = 0.5m +; change microcontroller +board_build.mcu = rp2350 +; change MCU frequency +board_build.f_cpu = 150000000L +; lib_deps = adafruit/Adafruit TinyUSB Library@^2.0.3 +build_flags = ${env.build_flags} -std=gnu99 -Wall -fdata-sections -ffunction-sections -fno-exceptions -Wl,--gc-sections -DRAM_ONLY_SETTINGS +debug_tool = cmsis-dap +lib_deps = + ${env.lib_deps} + SPI + Wire + EEPROM +lib_ignore = + HTTPUpdateServer + LittleFS + WiFi + WebServer + SerialBT + DNSServer + Hash + BluetoothSerial + +[env:RP2350-PICO2] +extends = common_rp2350 +board = generic_rp2350 +build_flags = ${common_rp2350.build_flags} -D BOARD=\"src/hal/boards/rp2350/boardmap_rpi_pico2.h\" +lib_deps = + ${env.lib_deps} + SPI + Wire + EEPROM + +[env:RP2350-CUSTOM] +extends = common_rp2350 +build_flags = ${common_rp2350.build_flags} -DMCU=MCU_CUSTOM +board = ${env.board} diff --git a/uCNC/src/hal/mcus/mcudefs.h b/uCNC/src/hal/mcus/mcudefs.h index e58a3160c..0216c936e 100644 --- a/uCNC/src/hal/mcus/mcudefs.h +++ b/uCNC/src/hal/mcus/mcudefs.h @@ -73,6 +73,13 @@ extern "C" #endif #endif +#if (MCU == MCU_RP2350) +#include "rp2350/mcumap_rp2350.h" +#ifndef CFG_TUSB_MCU +#define CFG_TUSB_MCU OPT_MCU_RP2350 +#endif +#endif + #if (MCU == MCU_VIRTUAL_WIN) #include "virtual/mcumap_virtual.h" #endif diff --git a/uCNC/src/hal/mcus/mcus.h b/uCNC/src/hal/mcus/mcus.h index e62e5de51..c8283daca 100644 --- a/uCNC/src/hal/mcus/mcus.h +++ b/uCNC/src/hal/mcus/mcus.h @@ -34,6 +34,7 @@ extern "C" #define MCU_ESP8266 40 #define MCU_ESP32 50 #define MCU_RP2040 60 +#define MCU_RP2350 61 #define MCU_VIRTUAL_WIN 99 #ifdef __cplusplus diff --git a/uCNC/src/hal/mcus/rp2040/rp2040_arduino.cpp b/uCNC/src/hal/mcus/rp2040/rp2040_arduino.cpp index deee16588..bb69a847f 100644 --- a/uCNC/src/hal/mcus/rp2040/rp2040_arduino.cpp +++ b/uCNC/src/hal/mcus/rp2040/rp2040_arduino.cpp @@ -16,7 +16,7 @@ See the GNU General Public License for more details. */ -#ifdef ARDUINO_ARCH_RP2040 +#if defined(ARDUINO_ARCH_RP2040) && !defined(TARGET_RP2350) #include #include #include diff --git a/uCNC/src/hal/mcus/rp2350/README.md b/uCNC/src/hal/mcus/rp2350/README.md new file mode 100644 index 000000000..e0d02c7ae --- /dev/null +++ b/uCNC/src/hal/mcus/rp2350/README.md @@ -0,0 +1,17 @@ +_µCNC for RP2040 can be built this way_ + +## Method one - PlatformIO (preferred) + +1. Get [Visual Studio Code](https://code.visualstudio.com/download) and install it. +2. Install the PlatformIO extension. +3. Open uCNC folder in VSCode. +4. Edit ```cnc_config.h file``` and ```cnc_hal_config.h file``` to fit your needs and board. +5. If needed edit the platformio.ini file environment for your board. Compile the sketch and upload it to your board. + +## Method two - Arduino IDE (easiest) + +1. Get [Arduino IDE](https://www.arduino.cc/en/software) and install it. +2. If you don't have install ESP8266 for Arduino with Arduino board manager has explained [here](https://github.com/earlephilhower/arduino-pico#installing-via-arduino-boards-manager) +3. Go to uCNC folder and open uCNC.ino sketch. +4. Edit ```cnc_config.h file``` and ```cnc_hal_config.h file``` to fit your needs and board. +5. Compile the sketch and upload it to your board. diff --git a/uCNC/src/hal/mcus/rp2350/ic74hc595.pio b/uCNC/src/hal/mcus/rp2350/ic74hc595.pio new file mode 100644 index 000000000..39fd590bf --- /dev/null +++ b/uCNC/src/hal/mcus/rp2350/ic74hc595.pio @@ -0,0 +1,57 @@ +; +;Name: ic74hc595.pio +;Description: Implements a custom PIO for 74HC595 shift register in µCNC for RP2040. +; +;Copyright: Copyright (c) João Martins +;Author: João Martins +;Date: 02-11-2023 +; +;µCNC is free software: you can redistribute it and/or modify +;it under the terms of the GNU General Public License as published by +;the Free Software Foundation, either version 3 of the License, or +;(at your option) any later version. Please see +; +;µCNC is distributed WITHOUT ANY WARRANTY; +;Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +;See the GNU General Public License for more details. +; + +.program ic74hc595 +.side_set 1 opt + +pull ; shift TX to OSR +set x, 31 ; load loop counter +set pins, 0 ; set all pins low (latch) +bitloop: ; loop bits +out pins, 1 side 0 ; Shift 1 bit from OSR to the data pin and the remaining down +jmp x-- bitloop side 1 ; decrement loop and bring clock up +set pins, 2 ; set 3rd pin (4=0b100) up (latch) + +% c-sdk { +#include "hardware/clocks.h" + +static inline void ic74hc595_program_init(PIO pio, uint sm, uint offset, uint pin_data, uint pin_clk, uint pin_latch, uint freq) { + pio_sm_config c = ic74hc595_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_data, 1); // define one pin to respond to the out instruction + sm_config_set_set_pins(&c, pin_clk, 2); // define all pins to respond to the set instruction + sm_config_set_sideset_pins(&c, pin_clk); // define clock as the side set base pin (and only) + // Only support MSB-first in this example code (shift to left, no auto push/pull, threshold=nbits) + sm_config_set_out_shift(&c, false, false, 32); + // All pins output + pio_sm_set_consecutive_pindirs(pio, sm, pin_data, 3, true); + pio_gpio_init(pio, pin_data); + pio_gpio_init(pio, pin_clk); + pio_gpio_init(pio, pin_latch); + // We only need TX, so get an 8-deep FIFO! + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (freq << 1); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} + +static inline void ic74hc595_program_write(PIO pio, uint sm, uint out) { + pio_sm_put_blocking(pio, sm, out); +} +%} \ No newline at end of file diff --git a/uCNC/src/hal/mcus/rp2350/ic74hc595.pio.h b/uCNC/src/hal/mcus/rp2350/ic74hc595.pio.h new file mode 100644 index 000000000..8afd0be2f --- /dev/null +++ b/uCNC/src/hal/mcus/rp2350/ic74hc595.pio.h @@ -0,0 +1,71 @@ +// -------------------------------------------------- // +// This file is autogenerated by pioasm; do not edit! // +// -------------------------------------------------- // + +#pragma once + +#if !PICO_NO_HARDWARE +#include "hardware/pio.h" +#endif + +// --------- // +// ic74hc595 // +// --------- // + +#define ic74hc595_wrap_target 0 +#define ic74hc595_wrap 5 + +static const uint16_t ic74hc595_program_instructions[] = { + // .wrap_target + 0x80a0, // 0: pull block + 0xe03f, // 1: set x, 31 + 0xe000, // 2: set pins, 0 + 0x7001, // 3: out pins, 1 side 0 + 0x1843, // 4: jmp x--, 3 side 1 + 0xe002, // 5: set pins, 2 + // .wrap +}; + +#if !PICO_NO_HARDWARE +static const struct pio_program ic74hc595_program = { + .instructions = ic74hc595_program_instructions, + .length = 6, + .origin = -1, +}; + +static inline pio_sm_config ic74hc595_program_get_default_config(uint offset) +{ + pio_sm_config c = pio_get_default_sm_config(); + sm_config_set_wrap(&c, offset + ic74hc595_wrap_target, offset + ic74hc595_wrap); + sm_config_set_sideset(&c, 2, true, false); + return c; +} + +#include "hardware/clocks.h" +static inline void ic74hc595_program_init(PIO pio, uint sm, uint offset, uint pin_data, uint pin_clk, uint pin_latch, uint freq) +{ + pio_sm_config c = ic74hc595_program_get_default_config(offset); + sm_config_set_out_pins(&c, pin_data, 1); // define one pin to respond to the out instruction + sm_config_set_set_pins(&c, pin_clk, 2); // define all pins to respond to the set instruction + sm_config_set_sideset_pins(&c, pin_clk); // define clock as the side set base pin (and only) + // Only support MSB-first in this example code (shift to left, no auto push/pull, threshold=nbits) + sm_config_set_out_shift(&c, false, false, 32); + // All pins output + pio_sm_set_consecutive_pindirs(pio, sm, pin_data, 3, true); + pio_gpio_init(pio, pin_data); + pio_gpio_init(pio, pin_clk); + pio_gpio_init(pio, pin_latch); + // We only need TX, so get an 8-deep FIFO! + sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX); + // SM transmits 1 bit per 8 execution cycles. + float div = (float)clock_get_hz(clk_sys) / (freq << 1); + sm_config_set_clkdiv(&c, div); + pio_sm_init(pio, sm, offset, &c); + pio_sm_set_enabled(pio, sm, true); +} +static inline void ic74hc595_program_write(PIO pio, uint sm, uint out) +{ + pio_sm_put_blocking(pio, sm, out); +} + +#endif diff --git a/uCNC/src/hal/mcus/rp2350/mcu_rp2350.c b/uCNC/src/hal/mcus/rp2350/mcu_rp2350.c new file mode 100644 index 000000000..93f4ab4ae --- /dev/null +++ b/uCNC/src/hal/mcus/rp2350/mcu_rp2350.c @@ -0,0 +1,935 @@ +/* + Name: mcu_rpi_pico.c + Description: Implements the µCNC HAL for ESP8266. + + Copyright: Copyright (c) João Martins + Author: João Martins + Date: 05-02-2022 + + µCNC is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. Please see + + µCNC is distributed WITHOUT ANY WARRANTY; + Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + See the GNU General Public License for more details. +*/ + +#include "../../../cnc.h" + +#if (MCU == MCU_RP2350) +#include +#include +#include + +static volatile bool rp2350_global_isr_enabled; + +extern void rp2350_uart_init(int baud); +extern void rp2350_uart_process(void); + +extern void rp2350_eeprom_init(int size); +extern uint8_t rp2350_eeprom_read(uint16_t address); +extern void rp2350_eeprom_write(uint16_t address, uint8_t value); +extern void rp2350_eeprom_flush(void); + +uint8_t rp2350_pwm[16]; + +#ifdef IC74HC595_CUSTOM_SHIFT_IO + +#ifndef IC74HC595_PIO_FREQ +#define IC74HC595_PIO_FREQ 20000000UL +#endif + +#if IC74HC595_COUNT != 4 +#error "IC74HC595_COUNT must be 4 to use ESP32 I2S mode for IO shifting" +#endif + +#ifdef IC74HC595_HAS_PWMS +#warning "IC74HC595 on RP2350 does not support soft PWM yet!" +#endif + +#include "pico/stdlib.h" +#include "hardware/pio.h" +#include "ic74hc595.pio.h" + +static PIO pio_ic74hc595; +static uint sm_ic74hc595; + +void ic74hc595_pio_init() +{ + pio_ic74hc595 = pio0; + sm_ic74hc595 = 0; + uint offset = pio_add_program(pio_ic74hc595, &ic74hc595_program); + ic74hc595_program_init(pio_ic74hc595, sm_ic74hc595, offset, IC74HC595_PIO_DATA, IC74HC595_PIO_CLK, IC74HC595_PIO_LATCH, IC74HC595_PIO_FREQ); +} + +// disable this function +// IO will be updated at a fixed rate +MCU_CALLBACK void ic74hc595_shift_io_pins(void) +{ + ic74hc595_program_write(pio_ic74hc595, sm_ic74hc595, *((volatile uint32_t *)&ic74hc595_io_pins[0])); +} + +#endif + +void mcu_din_isr(void) +{ + mcu_inputs_changed_cb(); +} + +void mcu_probe_isr(void) +{ + mcu_probe_changed_cb(); +} + +void mcu_limits_isr(void) +{ + mcu_limits_changed_cb(); +} + +void mcu_controls_isr(void) +{ + mcu_controls_changed_cb(); +} + +// RTC, ONESHOT, and SERVO alarms +// slow rate alarms all share a single timer +typedef struct rp2350_alarm_ +{ + uint32_t timeout; + void (*alarm_cb)(void); + struct rp2350_alarm_ *next; +} rp2350_alarm_t; + +volatile rp2350_alarm_t *mcu_alarms; +// the alarm isr processes all executes all pending RTC, ONESHOT and SERVO isr's +void mcu_alarm_isr(void) +{ + hw_clear_bits(&timer_hw->intr, (1U << ALARM_TIMER)); + if (mcu_alarms) + { + while (mcu_alarms->timeout < (uint32_t)timer_hw->timerawl) + { + rp2350_alarm_t *alarm = (rp2350_alarm_t *)mcu_alarms; + // advance + mcu_alarms = mcu_alarms->next; + // dequeue + alarm->next = NULL; + if (alarm->alarm_cb) + { + alarm->alarm_cb(); + } + + // no more alarms + if (!mcu_alarms) + { + return; + } + } + + timer_hw->alarm[ALARM_TIMER] = mcu_alarms->timeout; + } + else + { + // just re-arm + timer_hw->alarm[ALARM_TIMER] = 0xFFFFFFFF; + } +} + +// initializes the alarm isr +void mcu_alarms_init(void) +{ + hw_set_bits(&timer_hw->inte, 1u << ALARM_TIMER); + // Set irq handler for alarm irq + irq_set_exclusive_handler(ALARM_TIMER_IRQ, mcu_alarm_isr); + // Enable the alarm irq + irq_set_enabled(ALARM_TIMER_IRQ, true); + + // just re-arm + timer_hw->alarm[ALARM_TIMER] = 0xFFFFFFFF; +} + +// enqueues an alarm for execution +void mcu_enqueue_alarm(rp2350_alarm_t *a, uint32_t timeout_us) +{ + uint64_t target = timer_hw->timerawl + timeout_us; + a->timeout = (uint32_t)target; + a->next = NULL; + + __ATOMIC__ + { + rp2350_alarm_t *ptr = (rp2350_alarm_t *)mcu_alarms; + // is the only + if (!ptr) + { + mcu_alarms = a; + // adjust alarm to next event + timer_hw->alarm[ALARM_TIMER] = mcu_alarms->timeout; + } + else + { + while (ptr) + { + // comes before first alarm in queue + if (ptr->timeout < target && ptr->timeout < (uint32_t)target && ptr == mcu_alarms) + { + a->next = (rp2350_alarm_t *)mcu_alarms; + mcu_alarms = a; + break; + } + + // will be the last in queue + if (!ptr->next) + { + ptr->next = a; + break; + } + + // insert mid queue + if (ptr->next->timeout > target) + { + a->next = ptr->next; + ptr->next = a; + break; + } + + ptr = ptr->next; + } + } + } +} + +#if SERVOS_MASK > 0 + +static uint8_t mcu_servos[6]; +static rp2350_alarm_t servo_alarm; +#define servo_start_timeout(X) mcu_enqueue_alarm(&servo_alarm, (X)) + +static void mcu_clear_servos(void) +{ +#if ASSERT_PIN(SERVO0) + io_clear_output(SERVO0); +#endif +#if ASSERT_PIN(SERVO1) + io_clear_output(SERVO1); +#endif +#if ASSERT_PIN(SERVO2) + io_clear_output(SERVO2); +#endif +#if ASSERT_PIN(SERVO3) + io_clear_output(SERVO3); +#endif +#if ASSERT_PIN(SERVO4) + io_clear_output(SERVO4); +#endif +#if ASSERT_PIN(SERVO5) + io_clear_output(SERVO5); +#endif +} +#endif + +static rp2350_alarm_t rtc_alarm; + +void mcu_rtc_isr(void) +{ + // enqueue alarm again + mcu_enqueue_alarm(&rtc_alarm, 1000UL); + + // counts to 20 and reloads +#if SERVOS_MASK > 0 + static uint8_t ms_servo_counter = 0; + uint8_t servo_counter = ms_servo_counter; + + switch (servo_counter) + { +#if ASSERT_PIN(SERVO0) + case SERVO0_FRAME: + servo_start_timeout(mcu_servos[0]); + io_set_output(SERVO0); + break; +#endif +#if ASSERT_PIN(SERVO1) + case SERVO1_FRAME: + io_set_output(SERVO1); + servo_start_timeout(mcu_servos[1]); + break; +#endif +#if ASSERT_PIN(SERVO2) + case SERVO2_FRAME: + io_set_output(SERVO2); + servo_start_timeout(mcu_servos[2]); + break; +#endif +#if ASSERT_PIN(SERVO3) + case SERVO3_FRAME: + io_set_output(SERVO3); + servo_start_timeout(mcu_servos[3]); + break; +#endif +#if ASSERT_PIN(SERVO4) + case SERVO4_FRAME: + io_set_output(SERVO4); + servo_start_timeout(mcu_servos[4]); + break; +#endif +#if ASSERT_PIN(SERVO5) + case SERVO5_FRAME: + io_set_output(SERVO5); + servo_start_timeout(mcu_servos[5]); + break; +#endif + } + + servo_counter++; + ms_servo_counter = (servo_counter != 20) ? servo_counter : 0; + +#endif + mcu_rtc_cb(millis()); +} + +/** + * Multicore code + * **/ +void rp2350_core0_loop() +{ + rp2350_uart_process(); +} + +void setup1() +{ +} + +void loop1() +{ + for (;;) + { + cnc_run(); + } +} + +/** + * initializes the mcu + * this function needs to: + * - configure all IO pins (digital IO, PWM, Analog, etc...) + * - configure all interrupts + * - configure uart or usb + * - start the internal RTC + * */ +void mcu_init(void) +{ + mcu_io_init(); + +#ifdef IC74HC595_CUSTOM_SHIFT_IO + ic74hc595_pio_init(); +#endif +#ifndef RAM_ONLY_SETTINGS + rp2350_eeprom_init(NVM_STORAGE_SIZE); // 2K Emulated EEPROM +#endif + + rp2350_uart_init(BAUDRATE); + + pinMode(LED_BUILTIN, OUTPUT); + // init rtc, oneshot and servo alarms + mcu_alarms_init(); + rtc_alarm.alarm_cb = &mcu_rtc_isr; + mcu_enqueue_alarm(&rtc_alarm, 500000UL); + +#if SERVOS_MASK > 0 + servo_alarm.alarm_cb = &mcu_clear_servos; +#endif + +#ifdef MCU_HAS_SPI + spi_config_t spi_conf = {0}; + spi_conf.mode = SPI_MODE; + mcu_spi_config(spi_conf, SPI_FREQ); +#endif + +#ifdef MCU_HAS_SPI2 + spi_config_t spi2_conf = {0}; + spi2_conf.mode = SPI2_MODE; + mcu_spi2_config(spi2_conf, SPI2_FREQ); +#endif + +#ifdef MCU_HAS_I2C + mcu_i2c_config(I2C_FREQ); +#endif + +#ifdef MCU_HAS_ONESHOT + // // Set irq handler for alarm irq + // irq_set_exclusive_handler(ONESHOT_IRQ, mcu_oneshot_isr); + // // Enable the alarm irq + // irq_set_enabled(ITP_TIMER_IRQ, true); +#endif +} + +/** + * enables the pin probe mcu isr on change + * can be defined either as a function or a macro call + * */ +#ifndef mcu_enable_probe_isr +void mcu_enable_probe_isr(void) +{ +} +#endif + +/** + * disables the pin probe mcu isr on change + * can be defined either as a function or a macro call + * */ +#ifndef mcu_disable_probe_isr +void mcu_disable_probe_isr(void) +{ +} +#endif + +/** + * gets the voltage value of a built-in ADC pin + * can be defined either as a function or a macro call + * */ +#ifndef mcu_get_analog +uint16_t mcu_get_analog(uint8_t channel) +{ + return 0; +} +#endif + +/** + * sets the pwm value of a built-in pwm pin + * can be defined either as a function or a macro call + * */ +#ifndef mcu_set_pwm +void mcu_set_pwm(uint8_t pwm, uint8_t value) +{ +} +#endif + +/** + * gets the configured pwm value of a built-in pwm pin + * can be defined either as a function or a macro call + * */ +#ifndef mcu_get_pwm +uint8_t mcu_get_pwm(uint8_t pwm) +{ + return 0; +} +#endif + +// ISR +/** + * enables global interrupts on the MCU + * can be defined either as a function or a macro call + * */ +#ifndef mcu_enable_global_isr +void mcu_enable_global_isr(void) +{ + // ets_intr_unlock(); + rp2350_global_isr_enabled = true; +} +#endif + +/** + * disables global interrupts on the MCU + * can be defined either as a function or a macro call + * */ +#ifndef mcu_disable_global_isr +void mcu_disable_global_isr(void) +{ + rp2350_global_isr_enabled = false; + // ets_intr_lock(); +} +#endif + +/** + * gets global interrupts state on the MCU + * can be defined either as a function or a macro call + * */ +#ifndef mcu_get_global_isr +bool mcu_get_global_isr(void) +{ + return rp2350_global_isr_enabled; +} +#endif + +// Step interpolator + +static uint32_t mcu_step_counter; +static uint32_t mcu_step_reload; +static void mcu_itp_isr(void) +{ + static bool resetstep = false; + + mcu_disable_global_isr(); + // Clear the alarm irq + hw_clear_bits(&timer_hw->intr, (1U << ITP_TIMER)); + uint32_t target = (uint32_t)timer_hw->timerawl + mcu_step_reload; + timer_hw->alarm[ITP_TIMER] = target; + + if (!resetstep) + { + mcu_step_cb(); + } + + else + { + mcu_step_reset_cb(); + } + + resetstep = !resetstep; + mcu_enable_global_isr(); +} + +/** + * convert step rate to clock cycles + * */ +void mcu_freq_to_clocks(float frequency, uint16_t *ticks, uint16_t *prescaller) +{ + frequency = CLAMP((float)F_STEP_MIN, frequency, (float)F_STEP_MAX); + // up and down counter (generates half the step rate at each event) + uint32_t totalticks = (uint32_t)((float)(1000000UL >> 1) / frequency); + *prescaller = 1; + while (totalticks > 0xFFFF) + { + (*prescaller) += 1; + totalticks >>= 1; + } + + *ticks = (uint16_t)totalticks; +} + +float mcu_clocks_to_freq(uint16_t ticks, uint16_t prescaller) +{ + return ((float)(1000000UL >> 1) / (float)(((uint32_t)ticks) << prescaller)); +} + +/** + * starts the timer interrupt that generates the step pulses for the interpolator + * */ +void mcu_start_itp_isr(uint16_t ticks, uint16_t prescaller) +{ + hw_set_bits(&timer_hw->inte, 1u << ITP_TIMER); + // Set irq handler for alarm irq + irq_set_exclusive_handler(ITP_TIMER_IRQ, mcu_itp_isr); + // Enable the alarm irq + irq_set_enabled(ITP_TIMER_IRQ, true); + // Enable interrupt in block and at processor + + // Alarm is only 32 bits so if trying to delay more + // than that need to be careful and keep track of the upper + // bits + uint32_t target = (((uint32_t)ticks) << prescaller); + mcu_step_reload = target; + + target += (uint32_t)timer_hw->timerawl; + + // Write the lower 32 bits of the target time to the alarm which + // will arm it + timer_hw->alarm[ITP_TIMER] = target; +} + +/** + * changes the step rate of the timer interrupt that generates the step pulses for the interpolator + * */ +void mcu_change_itp_isr(uint16_t ticks, uint16_t prescaller) +{ + uint32_t target = (((uint32_t)ticks) << prescaller); + mcu_step_reload = target; + + target += (uint32_t)timer_hw->timerawl; + + // Write the lower 32 bits of the target time to the alarm which + // will arm it + timer_hw->alarm[ITP_TIMER] = target; +} + +/** + * stops the timer interrupt that generates the step pulses for the interpolator + * */ +void mcu_stop_itp_isr(void) +{ + irq_set_enabled(ITP_TIMER_IRQ, false); + hw_clear_bits(&timer_hw->inte, 1u << ITP_TIMER); +} + +/** + * runs all internal tasks of the MCU. + * for the moment these are: + * - if USB is enabled and MCU uses tinyUSB framework run tinyUSB tud_task + * */ +void mcu_dotasks(void) +{ +#ifndef RP2350_RUN_MULTICORE + rp2350_uart_process(); +#endif +} + +// Non volatile memory +/** + * gets a byte at the given EEPROM (or other non volatile memory) address of the MCU. + * */ +uint8_t mcu_eeprom_getc(uint16_t address) +{ + if (NVM_STORAGE_SIZE <= address) + { + DBGMSG("EEPROM invalid address @ %u", address); + return 0; + } +#ifndef RAM_ONLY_SETTINGS + return rp2350_eeprom_read(address); +#else + return 0; +#endif +} + +/** + * sets a byte at the given EEPROM (or other non volatile memory) address of the MCU. + * */ +void mcu_eeprom_putc(uint16_t address, uint8_t value) +{ + if (NVM_STORAGE_SIZE <= address) + { + DBGMSG("EEPROM invalid address @ %u", address); + } +#ifndef RAM_ONLY_SETTINGS + rp2350_eeprom_write(address, value); +#endif +} + +/** + * flushes all recorded registers into the eeprom. + * */ +void mcu_eeprom_flush(void) +{ +#ifndef RAM_ONLY_SETTINGS + rp2350_eeprom_flush(); +#endif +} + +#ifdef MCU_HAS_ONESHOT_TIMER +/** + * configures a single shot timeout in us + * */ + +static uint32_t rp2350_oneshot_reload; +static rp2350_alarm_t oneshot_alarm; +static void mcu_oneshot_isr(void) +{ + if (mcu_timeout_cb) + { + mcu_timeout_cb(); + } +} + +#ifndef mcu_config_timeout +void mcu_config_timeout(mcu_timeout_delgate fp, uint32_t timeout) +{ + // mcu_timeout_cb = fp; + oneshot_alarm.alarm_cb = &mcu_oneshot_isr; + rp2350_oneshot_reload = (1000000UL / timeout); + mcu_timeout_cb = fp; +} +#endif + +/** + * starts the timeout. Once hit the the respective callback is called + * */ +#ifndef mcu_start_timeout +void mcu_start_timeout() +{ + // hw_set_bits(&timer_hw->inte, 1u << ONESHOT_TIMER); + // // Enable interrupt in block and at processor + // // Alarm is only 32 bits so if trying to delay more + // // than that need to be careful and keep track of the upper + // // bits + // uint32_t target = (((uint32_t)ticks) << prescaller); + // rp2350_oneshot_reload = target; + + // target += (uint32_t)timer_hw->timerawl; + + // // Write the lower 32 bits of the target time to the alarm which + // // will arm it + // timer_hw->alarm[ONESHOT_TIMER] = target; + mcu_enqueue_alarm(&oneshot_alarm, rp2350_oneshot_reload); +} +#endif +#endif + +/** + * + * This handles SPI communications + * + * **/ + +#if defined(MCU_HAS_SPI) && !defined(USE_ARDUINO_SPI_LIBRARY) +#include +#include +#include "pico/stdlib.h" +#include "pico/binary_info.h" + +static spi_config_t rp2350_spi_config; + +void mcu_spi_init(void) +{ + spi_init(SPI_HW, SPI_FREQ); + // Enable SPI 0 at 1 MHz and connect to GPIOs + gpio_set_function(SPI_CLK_BIT, GPIO_FUNC_SPI); + gpio_set_function(SPI_SDO_BIT, GPIO_FUNC_SPI); + gpio_set_function(SPI_SDI_BIT, GPIO_FUNC_SPI); +#ifdef SPI_CS_BIT + gpio_init(SPI_CS_BIT); +#endif +} + +void mcu_spi_config(spi_config_t config, uint32_t frequency) +{ + rp2350_spi_config = config; + spi_deinit(SPI_HW); + spi_set_baudrate(SPI_HW, frequency); + spi_set_format(SPI_HW, 8, ((config.mode >> 1) & 0x01), (config.mode & 0x01), SPI_MSB_FIRST); + mcu_spi_init(); +} + +uint8_t mcu_spi_xmit(uint8_t data) +{ + spi_get_hw(SPI_HW)->dr = data; + while ((spi_get_hw(SPI_HW)->sr & SPI_SSPSR_BSY_BITS)) + ; + return (spi_get_hw(SPI_HW)->dr & 0xFF); +} + +void mcu_spi_start(spi_config_t config, uint32_t frequency) +{ + mcu_spi_config(config, frequency); +} + +void mcu_spi_stop(void) +{ +} + +#ifndef BULK_SPI_TIMEOUT +#define BULK_SPI_TIMEOUT (1000 / INTERPOLATOR_FREQ) +#endif + +bool mcu_spi_bulk_transfer(const uint8_t *out, uint8_t *in, uint16_t len) +{ + static bool transmitting = false; + // Grab some unused dma channels + static uint dma_tx = 0; + static uint dma_rx = 0; + + if (rp2350_spi_config.enable_dma) + { + if (!transmitting) + { + uint32_t startmask = (1u << dma_tx); + // We set the outbound DMA to transfer from a memory buffer to the SPI transmit FIFO paced by the SPI TX FIFO DREQ + // The default is for the read address to increment every element (in this case 1 byte = DMA_SIZE_8) + // and for the write address to remain unchanged. + dma_tx = dma_claim_unused_channel(true); + + dma_channel_config c = dma_channel_get_default_config(dma_tx); + channel_config_set_transfer_data_size(&c, DMA_SIZE_8); + channel_config_set_dreq(&c, spi_get_dreq(spi_default, true)); + dma_channel_configure(dma_tx, &c, + &spi_get_hw(SPI_HW)->dr, // write address + out, // read address + len, // element count (each element is of size transfer_data_size) + false); // don't start yet + + if (in) + { + // We set the inbound DMA to transfer from the SPI receive FIFO to a memory buffer paced by the SPI RX FIFO DREQ + // We configure the read address to remain unchanged for each element, but the write + // address to increment (so data is written throughout the buffer) + dma_rx = dma_claim_unused_channel(true); + c = dma_channel_get_default_config(dma_rx); + channel_config_set_transfer_data_size(&c, DMA_SIZE_8); + channel_config_set_dreq(&c, spi_get_dreq(spi_default, false)); + channel_config_set_read_increment(&c, false); + channel_config_set_write_increment(&c, true); + dma_channel_configure(dma_rx, &c, + in, // write address + &spi_get_hw(SPI_HW)->dr, // read address + len, // element count (each element is of size transfer_data_size) + false); // don't start yet + + startmask |= (1u << dma_rx); + } + + // start the DMA transmission + dma_start_channel_mask(startmask); + transmitting = true; + } + else + { + if (!dma_channel_is_busy(dma_tx) && !dma_channel_is_busy(dma_rx)) + { + dma_channel_unclaim(dma_tx); + dma_channel_unclaim(dma_rx); + transmitting = false; + } + } + } + else + { + transmitting = false; + uint32_t timeout = BULK_SPI_TIMEOUT + mcu_millis(); + while (len--) + { + uint8_t c = mcu_spi_xmit(*out++); + if (in) + { + *in++ = c; + } + + if (timeout < mcu_millis()) + { + timeout = BULK_SPI_TIMEOUT + mcu_millis(); + cnc_dotasks(); + } + } + + return false; + } + + return transmitting; +} +#endif + +#if defined(MCU_HAS_SPI2) && !defined(USE_ARDUINO_SPI_LIBRARY) +#include +#include +#include "pico/stdlib.h" +#include "pico/binary_info.h" + +static spi_config_t rp2350_spi2_config; + +void mcu_spi2_init(void) +{ + spi_init(SPI2_HW, SPI2_FREQ); + // Enable SPI 0 at 1 MHz and connect to GPIOs + gpio_set_function(SPI2_CLK_BIT, GPIO_FUNC_SPI); + gpio_set_function(SPI2_SDO_BIT, GPIO_FUNC_SPI); + gpio_set_function(SPI2_SDI_BIT, GPIO_FUNC_SPI); +#ifdef SPI2_CS_BIT + gpio_init(SPI2_CS_BIT); +#endif +} + +void mcu_spi2_config(spi_config_t config, uint32_t frequency) +{ + rp2350_spi2_config = config; + spi_deinit(SPI2_HW); + spi_set_baudrate(SPI2_HW, frequency); + spi_set_format(SPI2_HW, 8, ((config.mode >> 1) & 0x01), (config.mode & 0x01), SPI_MSB_FIRST); + mcu_spi2_init(); +} + +uint8_t mcu_spi2_xmit(uint8_t data) +{ + spi_get_hw(SPI2_HW)->dr = data; + while ((spi_get_hw(SPI2_HW)->sr & SPI_SSPSR_BSY_BITS)) + ; + return (spi_get_hw(SPI2_HW)->dr & 0xFF); +} + +void mcu_spi2_start(spi_config_t config, uint32_t frequency) +{ + mcu_spi2_config(config, frequency); +} + +void mcu_spi2_stop(void) +{ +} + +#ifndef BULK_SPI2_TIMEOUT +#define BULK_SPI2_TIMEOUT (1000 / INTERPOLATOR_FREQ) +#endif + +bool mcu_spi2_bulk_transfer(const uint8_t *out, uint8_t *in, uint16_t len) +{ + static bool transmitting = false; + // Grab some unused dma channels + static uint dma_tx = 0; + static uint dma_rx = 0; + + if (rp2350_spi2_config.enable_dma) + { + if (!transmitting) + { + uint32_t startmask = (1u << dma_tx); + // We set the outbound DMA to transfer from a memory buffer to the SPI transmit FIFO paced by the SPI TX FIFO DREQ + // The default is for the read address to increment every element (in this case 1 byte = DMA_SIZE_8) + // and for the write address to remain unchanged. + dma_tx = dma_claim_unused_channel(true); + + dma_channel_config c = dma_channel_get_default_config(dma_tx); + channel_config_set_transfer_data_size(&c, DMA_SIZE_8); + channel_config_set_dreq(&c, spi_get_dreq(spi_default, true)); + dma_channel_configure(dma_tx, &c, + &spi_get_hw(SPI2_HW)->dr, // write address + out, // read address + len, // element count (each element is of size transfer_data_size) + false); // don't start yet + + if (in) + { + // We set the inbound DMA to transfer from the SPI receive FIFO to a memory buffer paced by the SPI RX FIFO DREQ + // We configure the read address to remain unchanged for each element, but the write + // address to increment (so data is written throughout the buffer) + dma_rx = dma_claim_unused_channel(true); + c = dma_channel_get_default_config(dma_rx); + channel_config_set_transfer_data_size(&c, DMA_SIZE_8); + channel_config_set_dreq(&c, spi_get_dreq(spi_default, false)); + channel_config_set_read_increment(&c, false); + channel_config_set_write_increment(&c, true); + dma_channel_configure(dma_rx, &c, + in, // write address + &spi_get_hw(SPI2_HW)->dr, // read address + len, // element count (each element is of size transfer_data_size) + false); // don't start yet + + startmask |= (1u << dma_rx); + } + + // start the DMA transmission + dma_start_channel_mask(startmask); + transmitting = true; + } + else + { + if (!dma_channel_is_busy(dma_tx) && !dma_channel_is_busy(dma_rx)) + { + dma_channel_unclaim(dma_tx); + dma_channel_unclaim(dma_rx); + transmitting = false; + } + } + } + else + { + transmitting = false; + uint32_t timeout = BULK_SPI2_TIMEOUT + mcu_millis(); + while (len--) + { + uint8_t c = mcu_spi2_xmit(*out++); + if (in) + { + *in++ = c; + } + + if (timeout < mcu_millis()) + { + timeout = BULK_SPI2_TIMEOUT + mcu_millis(); + cnc_dotasks(); + } + } + + return false; + } + + return transmitting; +} +#endif + +#endif diff --git a/uCNC/src/hal/mcus/rp2350/mcumap_rp2350.h b/uCNC/src/hal/mcus/rp2350/mcumap_rp2350.h new file mode 100644 index 000000000..c0474d633 --- /dev/null +++ b/uCNC/src/hal/mcus/rp2350/mcumap_rp2350.h @@ -0,0 +1,1467 @@ +/* + Name: mcumap_rp2350.h + Description: Contains all MCU and PIN definitions for RP2350 to run µCNC. + + Copyright: Copyright (c) João Martins + Author: João Martins + Date: 16-01-2023 + + µCNC is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. Please see + + µCNC is distributed WITHOUT ANY WARRANTY; + Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + See the GNU General Public License for more details. +*/ + +#ifndef MCUMAP_RP2350_H +#define MCUMAP_RP2350_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include +#include + +/* + Generates all the interface definitions. + This creates a middle HAL layer between the board IO pins and the AVR funtionalities +*/ +/* + MCU specific definitions and replacements +*/ + +/* + RP2350 Defaults +*/ +// defines the frequency of the mcu +#ifndef F_CPU +#define F_CPU 133000000L +#endif +// defines the maximum and minimum step rates +#ifndef F_STEP_MAX +#define F_STEP_MAX 64000 +#endif +#ifndef F_STEP_MIN +#define F_STEP_MIN 1 +#endif + +#ifndef RP2350 +#define RP2350 +#endif + +// defines special mcu to access flash strings and arrays +#define __rom__ +#define __romstr__ +#define rom_strptr * +#define rom_strcpy strcpy +#define rom_strncpy strncpy +#define rom_memcpy memcpy +#define rom_read_byte * + +// needed by software delays +// this can be ignored since custom delay functions will be defined +#ifndef MCU_CLOCKS_PER_CYCLE +#define MCU_CLOCKS_PER_CYCLE 1 +#endif +#ifndef MCU_CYCLES_PER_LOOP +#define MCU_CYCLES_PER_LOOP 1 +#endif +#ifndef MCU_CYCLES_PER_LOOP_OVERHEAD +#define MCU_CYCLES_PER_LOOP_OVERHEAD 0 +#endif + + // this next set of rules defines the internal delay macros + // #define F_CPU_MHZ (F_CPU / 1000000UL) + // #define US_TO_CYCLES(X) (X * F_CPU_MHZ) + + // extern unsigned long ulMainGetRunTimeCounterValue(); + /* + #define mcu_delay_cycles(X) \ + { \ + uint32_t target = ulMainGetRunTimeCounterValue() + (X); \ + while (target > ulMainGetRunTimeCounterValue()) \ + ; \ + } + */ + // #define mcu_delay_100ns() mcu_delay_cycles(F_CPU_MHZ / 10UL) + // #define mcu_delay_us(X) (mcu_delay_cycles(US_TO_CYCLES(X))) + +#ifdef RX_BUFFER_CAPACITY +#define RX_BUFFER_CAPACITY 255 +#endif + +#define __SIZEOF_FLOAT__ 4 + +// used by the parser +// this method is faster then normal multiplication (for 32 bit for 16 and 8 bits is slightly lower) +// overrides utils.h definition to implement this method with or without fast math option enabled +#define fast_int_mul10(x) ((((x) << 2) + (x)) << 1) + +// IO pins +#if (defined(STEP0_BIT)) +#define DIO1 1 +#define STEP0 1 +#define DIO1_BIT (STEP0_BIT) +#endif +#if (defined(STEP1_BIT)) +#define DIO2 2 +#define STEP1 2 +#define DIO2_BIT (STEP1_BIT) +#endif +#if (defined(STEP2_BIT)) +#define DIO3 3 +#define STEP2 3 +#define DIO3_BIT (STEP2_BIT) +#endif +#if (defined(STEP3_BIT)) +#define DIO4 4 +#define STEP3 4 +#define DIO4_BIT (STEP3_BIT) +#endif +#if (defined(STEP4_BIT)) +#define DIO5 5 +#define STEP4 5 +#define DIO5_BIT (STEP4_BIT) +#endif +#if (defined(STEP5_BIT)) +#define DIO6 6 +#define STEP5 6 +#define DIO6_BIT (STEP5_BIT) +#endif +#if (defined(STEP6_BIT)) +#define DIO7 7 +#define STEP6 7 +#define DIO7_BIT (STEP6_BIT) +#endif +#if (defined(STEP7_BIT)) +#define DIO8 8 +#define STEP7 8 +#define DIO8_BIT (STEP7_BIT) +#endif +#if (defined(DIR0_BIT)) +#define DIO9 9 +#define DIR0 9 +#define DIO9_BIT (DIR0_BIT) +#endif +#if (defined(DIR1_BIT)) +#define DIO10 10 +#define DIR1 10 +#define DIO10_BIT (DIR1_BIT) +#endif +#if (defined(DIR2_BIT)) +#define DIO11 11 +#define DIR2 11 +#define DIO11_BIT (DIR2_BIT) +#endif +#if (defined(DIR3_BIT)) +#define DIO12 12 +#define DIR3 12 +#define DIO12_BIT (DIR3_BIT) +#endif +#if (defined(DIR4_BIT)) +#define DIO13 13 +#define DIR4 13 +#define DIO13_BIT (DIR4_BIT) +#endif +#if (defined(DIR5_BIT)) +#define DIO14 14 +#define DIR5 14 +#define DIO14_BIT (DIR5_BIT) +#endif +#if (defined(DIR6_BIT)) +#define DIO15 15 +#define DIR6 15 +#define DIO15_BIT (DIR6_BIT) +#endif +#if (defined(DIR7_BIT)) +#define DIO16 16 +#define DIR7 16 +#define DIO16_BIT (DIR7_BIT) +#endif +#if (defined(STEP0_EN_BIT)) +#define DIO17 17 +#define STEP0_EN 17 +#define DIO17_BIT (STEP0_EN_BIT) +#endif +#if (defined(STEP1_EN_BIT)) +#define DIO18 18 +#define STEP1_EN 18 +#define DIO18_BIT (STEP1_EN_BIT) +#endif +#if (defined(STEP2_EN_BIT)) +#define DIO19 19 +#define STEP2_EN 19 +#define DIO19_BIT (STEP2_EN_BIT) +#endif +#if (defined(STEP3_EN_BIT)) +#define DIO20 20 +#define STEP3_EN 20 +#define DIO20_BIT (STEP3_EN_BIT) +#endif +#if (defined(STEP4_EN_BIT)) +#define DIO21 21 +#define STEP4_EN 21 +#define DIO21_BIT (STEP4_EN_BIT) +#endif +#if (defined(STEP5_EN_BIT)) +#define DIO22 22 +#define STEP5_EN 22 +#define DIO22_BIT (STEP5_EN_BIT) +#endif +#if (defined(STEP6_EN_BIT)) +#define DIO23 23 +#define STEP6_EN 23 +#define DIO23_BIT (STEP6_EN_BIT) +#endif +#if (defined(STEP7_EN_BIT)) +#define DIO24 24 +#define STEP7_EN 24 +#define DIO24_BIT (STEP7_EN_BIT) +#endif +#if (defined(PWM0_BIT)) +#define DIO25 25 +#define PWM0 25 +#define DIO25_BIT (PWM0_BIT) +#endif +#if (defined(PWM1_BIT)) +#define DIO26 26 +#define PWM1 26 +#define DIO26_BIT (PWM1_BIT) +#endif +#if (defined(PWM2_BIT)) +#define DIO27 27 +#define PWM2 27 +#define DIO27_BIT (PWM2_BIT) +#endif +#if (defined(PWM3_BIT)) +#define DIO28 28 +#define PWM3 28 +#define DIO28_BIT (PWM3_BIT) +#endif +#if (defined(PWM4_BIT)) +#define DIO29 29 +#define PWM4 29 +#define DIO29_BIT (PWM4_BIT) +#endif +#if (defined(PWM5_BIT)) +#define DIO30 30 +#define PWM5 30 +#define DIO30_BIT (PWM5_BIT) +#endif +#if (defined(PWM6_BIT)) +#define DIO31 31 +#define PWM6 31 +#define DIO31_BIT (PWM6_BIT) +#endif +#if (defined(PWM7_BIT)) +#define DIO32 32 +#define PWM7 32 +#define DIO32_BIT (PWM7_BIT) +#endif +#if (defined(PWM8_BIT)) +#define DIO33 33 +#define PWM8 33 +#define DIO33_BIT (PWM8_BIT) +#endif +#if (defined(PWM9_BIT)) +#define DIO34 34 +#define PWM9 34 +#define DIO34_BIT (PWM9_BIT) +#endif +#if (defined(PWM10_BIT)) +#define DIO35 35 +#define PWM10 35 +#define DIO35_BIT (PWM10_BIT) +#endif +#if (defined(PWM11_BIT)) +#define DIO36 36 +#define PWM11 36 +#define DIO36_BIT (PWM11_BIT) +#endif +#if (defined(PWM12_BIT)) +#define DIO37 37 +#define PWM12 37 +#define DIO37_BIT (PWM12_BIT) +#endif +#if (defined(PWM13_BIT)) +#define DIO38 38 +#define PWM13 38 +#define DIO38_BIT (PWM13_BIT) +#endif +#if (defined(PWM14_BIT)) +#define DIO39 39 +#define PWM14 39 +#define DIO39_BIT (PWM14_BIT) +#endif +#if (defined(PWM15_BIT)) +#define DIO40 40 +#define PWM15 40 +#define DIO40_BIT (PWM15_BIT) +#endif +#if (defined(SERVO0_BIT)) +#define DIO41 41 +#define SERVO0 41 +#define DIO41_BIT (SERVO0_BIT) +#endif +#if (defined(SERVO1_BIT)) +#define DIO42 42 +#define SERVO1 42 +#define DIO42_BIT (SERVO1_BIT) +#endif +#if (defined(SERVO2_BIT)) +#define DIO43 43 +#define SERVO2 43 +#define DIO43_BIT (SERVO2_BIT) +#endif +#if (defined(SERVO3_BIT)) +#define DIO44 44 +#define SERVO3 44 +#define DIO44_BIT (SERVO3_BIT) +#endif +#if (defined(SERVO4_BIT)) +#define DIO45 45 +#define SERVO4 45 +#define DIO45_BIT (SERVO4_BIT) +#endif +#if (defined(SERVO5_BIT)) +#define DIO46 46 +#define SERVO5 46 +#define DIO46_BIT (SERVO5_BIT) +#endif +#if (defined(DOUT0_BIT)) +#define DIO47 47 +#define DOUT0 47 +#define DIO47_BIT (DOUT0_BIT) +#endif +#if (defined(DOUT1_BIT)) +#define DIO48 48 +#define DOUT1 48 +#define DIO48_BIT (DOUT1_BIT) +#endif +#if (defined(DOUT2_BIT)) +#define DIO49 49 +#define DOUT2 49 +#define DIO49_BIT (DOUT2_BIT) +#endif +#if (defined(DOUT3_BIT)) +#define DIO50 50 +#define DOUT3 50 +#define DIO50_BIT (DOUT3_BIT) +#endif +#if (defined(DOUT4_BIT)) +#define DIO51 51 +#define DOUT4 51 +#define DIO51_BIT (DOUT4_BIT) +#endif +#if (defined(DOUT5_BIT)) +#define DIO52 52 +#define DOUT5 52 +#define DIO52_BIT (DOUT5_BIT) +#endif +#if (defined(DOUT6_BIT)) +#define DIO53 53 +#define DOUT6 53 +#define DIO53_BIT (DOUT6_BIT) +#endif +#if (defined(DOUT7_BIT)) +#define DIO54 54 +#define DOUT7 54 +#define DIO54_BIT (DOUT7_BIT) +#endif +#if (defined(DOUT8_BIT)) +#define DIO55 55 +#define DOUT8 55 +#define DIO55_BIT (DOUT8_BIT) +#endif +#if (defined(DOUT9_BIT)) +#define DIO56 56 +#define DOUT9 56 +#define DIO56_BIT (DOUT9_BIT) +#endif +#if (defined(DOUT10_BIT)) +#define DIO57 57 +#define DOUT10 57 +#define DIO57_BIT (DOUT10_BIT) +#endif +#if (defined(DOUT11_BIT)) +#define DIO58 58 +#define DOUT11 58 +#define DIO58_BIT (DOUT11_BIT) +#endif +#if (defined(DOUT12_BIT)) +#define DIO59 59 +#define DOUT12 59 +#define DIO59_BIT (DOUT12_BIT) +#endif +#if (defined(DOUT13_BIT)) +#define DIO60 60 +#define DOUT13 60 +#define DIO60_BIT (DOUT13_BIT) +#endif +#if (defined(DOUT14_BIT)) +#define DIO61 61 +#define DOUT14 61 +#define DIO61_BIT (DOUT14_BIT) +#endif +#if (defined(DOUT15_BIT)) +#define DIO62 62 +#define DOUT15 62 +#define DIO62_BIT (DOUT15_BIT) +#endif +#if (defined(DOUT16_BIT)) +#define DIO63 63 +#define DOUT16 63 +#define DIO63_BIT (DOUT16_BIT) +#endif +#if (defined(DOUT17_BIT)) +#define DIO64 64 +#define DOUT17 64 +#define DIO64_BIT (DOUT17_BIT) +#endif +#if (defined(DOUT18_BIT)) +#define DIO65 65 +#define DOUT18 65 +#define DIO65_BIT (DOUT18_BIT) +#endif +#if (defined(DOUT19_BIT)) +#define DIO66 66 +#define DOUT19 66 +#define DIO66_BIT (DOUT19_BIT) +#endif +#if (defined(DOUT20_BIT)) +#define DIO67 67 +#define DOUT20 67 +#define DIO67_BIT (DOUT20_BIT) +#endif +#if (defined(DOUT21_BIT)) +#define DIO68 68 +#define DOUT21 68 +#define DIO68_BIT (DOUT21_BIT) +#endif +#if (defined(DOUT22_BIT)) +#define DIO69 69 +#define DOUT22 69 +#define DIO69_BIT (DOUT22_BIT) +#endif +#if (defined(DOUT23_BIT)) +#define DIO70 70 +#define DOUT23 70 +#define DIO70_BIT (DOUT23_BIT) +#endif +#if (defined(DOUT24_BIT)) +#define DIO71 71 +#define DOUT24 71 +#define DIO71_BIT (DOUT24_BIT) +#endif +#if (defined(DOUT25_BIT)) +#define DIO72 72 +#define DOUT25 72 +#define DIO72_BIT (DOUT25_BIT) +#endif +#if (defined(DOUT26_BIT)) +#define DIO73 73 +#define DOUT26 73 +#define DIO73_BIT (DOUT26_BIT) +#endif +#if (defined(DOUT27_BIT)) +#define DIO74 74 +#define DOUT27 74 +#define DIO74_BIT (DOUT27_BIT) +#endif +#if (defined(DOUT28_BIT)) +#define DIO75 75 +#define DOUT28 75 +#define DIO75_BIT (DOUT28_BIT) +#endif +#if (defined(DOUT29_BIT)) +#define DIO76 76 +#define DOUT29 76 +#define DIO76_BIT (DOUT29_BIT) +#endif +#if (defined(DOUT30_BIT)) +#define DIO77 77 +#define DOUT30 77 +#define DIO77_BIT (DOUT30_BIT) +#endif +#if (defined(DOUT31_BIT)) +#define DIO78 78 +#define DOUT31 78 +#define DIO78_BIT (DOUT31_BIT) +#endif +#if(defined(DOUT32_BIT)) +#define DIO79 79 +#define DOUT32 79 +#define DIO79_BIT (DOUT32_BIT) +#endif +#if(defined(DOUT33_BIT)) +#define DIO80 80 +#define DOUT33 80 +#define DIO80_BIT (DOUT33_BIT) +#endif +#if(defined(DOUT34_BIT)) +#define DIO81 81 +#define DOUT34 81 +#define DIO81_BIT (DOUT34_BIT) +#endif +#if(defined(DOUT35_BIT)) +#define DIO82 82 +#define DOUT35 82 +#define DIO82_BIT (DOUT35_BIT) +#endif +#if(defined(DOUT36_BIT)) +#define DIO83 83 +#define DOUT36 83 +#define DIO83_BIT (DOUT36_BIT) +#endif +#if(defined(DOUT37_BIT)) +#define DIO84 84 +#define DOUT37 84 +#define DIO84_BIT (DOUT37_BIT) +#endif +#if(defined(DOUT38_BIT)) +#define DIO85 85 +#define DOUT38 85 +#define DIO85_BIT (DOUT38_BIT) +#endif +#if(defined(DOUT39_BIT)) +#define DIO86 86 +#define DOUT39 86 +#define DIO86_BIT (DOUT39_BIT) +#endif +#if(defined(DOUT40_BIT)) +#define DIO87 87 +#define DOUT40 87 +#define DIO87_BIT (DOUT40_BIT) +#endif +#if(defined(DOUT41_BIT)) +#define DIO88 88 +#define DOUT41 88 +#define DIO88_BIT (DOUT41_BIT) +#endif +#if(defined(DOUT42_BIT)) +#define DIO89 89 +#define DOUT42 89 +#define DIO89_BIT (DOUT42_BIT) +#endif +#if(defined(DOUT43_BIT)) +#define DIO90 90 +#define DOUT43 90 +#define DIO90_BIT (DOUT43_BIT) +#endif +#if(defined(DOUT44_BIT)) +#define DIO91 91 +#define DOUT44 91 +#define DIO91_BIT (DOUT44_BIT) +#endif +#if(defined(DOUT45_BIT)) +#define DIO92 92 +#define DOUT45 92 +#define DIO92_BIT (DOUT45_BIT) +#endif +#if(defined(DOUT46_BIT)) +#define DIO93 93 +#define DOUT46 93 +#define DIO93_BIT (DOUT46_BIT) +#endif +#if(defined(DOUT47_BIT)) +#define DIO94 94 +#define DOUT47 94 +#define DIO94_BIT (DOUT47_BIT) +#endif +#if(defined(DOUT48_BIT)) +#define DIO95 95 +#define DOUT48 95 +#define DIO95_BIT (DOUT48_BIT) +#endif +#if(defined(DOUT49_BIT)) +#define DIO96 96 +#define DOUT49 96 +#define DIO96_BIT (DOUT49_BIT) +#endif +#if (defined(LIMIT_X_BIT)) +#define DIO100 100 +#define LIMIT_X 100 +#define DIO100_BIT (LIMIT_X_BIT) +#endif +#if (defined(LIMIT_Y_BIT)) +#define DIO101 101 +#define LIMIT_Y 101 +#define DIO101_BIT (LIMIT_Y_BIT) +#endif +#if (defined(LIMIT_Z_BIT)) +#define DIO102 102 +#define LIMIT_Z 102 +#define DIO102_BIT (LIMIT_Z_BIT) +#endif +#if (defined(LIMIT_X2_BIT)) +#define DIO103 103 +#define LIMIT_X2 103 +#define DIO103_BIT (LIMIT_X2_BIT) +#endif +#if (defined(LIMIT_Y2_BIT)) +#define DIO104 104 +#define LIMIT_Y2 104 +#define DIO104_BIT (LIMIT_Y2_BIT) +#endif +#if (defined(LIMIT_Z2_BIT)) +#define DIO105 105 +#define LIMIT_Z2 105 +#define DIO105_BIT (LIMIT_Z2_BIT) +#endif +#if (defined(LIMIT_A_BIT)) +#define DIO106 106 +#define LIMIT_A 106 +#define DIO106_BIT (LIMIT_A_BIT) +#endif +#if (defined(LIMIT_B_BIT)) +#define DIO107 107 +#define LIMIT_B 107 +#define DIO107_BIT (LIMIT_B_BIT) +#endif +#if (defined(LIMIT_C_BIT)) +#define DIO108 108 +#define LIMIT_C 108 +#define DIO108_BIT (LIMIT_C_BIT) +#endif +#if (defined(PROBE_BIT)) +#define DIO109 109 +#define PROBE 109 +#define DIO109_BIT (PROBE_BIT) +#endif +#if (defined(ESTOP_BIT)) +#define DIO110 110 +#define ESTOP 110 +#define DIO110_BIT (ESTOP_BIT) +#endif +#if (defined(SAFETY_DOOR_BIT)) +#define DIO111 111 +#define SAFETY_DOOR 111 +#define DIO111_BIT (SAFETY_DOOR_BIT) +#endif +#if (defined(FHOLD_BIT)) +#define DIO112 112 +#define FHOLD 112 +#define DIO112_BIT (FHOLD_BIT) +#endif +#if (defined(CS_RES_BIT)) +#define DIO113 113 +#define CS_RES 113 +#define DIO113_BIT (CS_RES_BIT) +#endif +#if (defined(ANALOG0_BIT)) +#define DIO114 114 +#define ANALOG0 114 +#define DIO114_BIT (ANALOG0_BIT) +#endif +#if (defined(ANALOG1_BIT)) +#define DIO115 115 +#define ANALOG1 115 +#define DIO115_BIT (ANALOG1_BIT) +#endif +#if (defined(ANALOG2_BIT)) +#define DIO116 116 +#define ANALOG2 116 +#define DIO116_BIT (ANALOG2_BIT) +#endif +#if (defined(ANALOG3_BIT)) +#define DIO117 117 +#define ANALOG3 117 +#define DIO117_BIT (ANALOG3_BIT) +#endif +#if (defined(ANALOG4_BIT)) +#define DIO118 118 +#define ANALOG4 118 +#define DIO118_BIT (ANALOG4_BIT) +#endif +#if (defined(ANALOG5_BIT)) +#define DIO119 119 +#define ANALOG5 119 +#define DIO119_BIT (ANALOG5_BIT) +#endif +#if (defined(ANALOG6_BIT)) +#define DIO120 120 +#define ANALOG6 120 +#define DIO120_BIT (ANALOG6_BIT) +#endif +#if (defined(ANALOG7_BIT)) +#define DIO121 121 +#define ANALOG7 121 +#define DIO121_BIT (ANALOG7_BIT) +#endif +#if (defined(ANALOG8_BIT)) +#define DIO122 122 +#define ANALOG8 122 +#define DIO122_BIT (ANALOG8_BIT) +#endif +#if (defined(ANALOG9_BIT)) +#define DIO123 123 +#define ANALOG9 123 +#define DIO123_BIT (ANALOG9_BIT) +#endif +#if (defined(ANALOG10_BIT)) +#define DIO124 124 +#define ANALOG10 124 +#define DIO124_BIT (ANALOG10_BIT) +#endif +#if (defined(ANALOG11_BIT)) +#define DIO125 125 +#define ANALOG11 125 +#define DIO125_BIT (ANALOG11_BIT) +#endif +#if (defined(ANALOG12_BIT)) +#define DIO126 126 +#define ANALOG12 126 +#define DIO126_BIT (ANALOG12_BIT) +#endif +#if (defined(ANALOG13_BIT)) +#define DIO127 127 +#define ANALOG13 127 +#define DIO127_BIT (ANALOG13_BIT) +#endif +#if (defined(ANALOG14_BIT)) +#define DIO128 128 +#define ANALOG14 128 +#define DIO128_BIT (ANALOG14_BIT) +#endif +#if (defined(ANALOG15_BIT)) +#define DIO129 129 +#define ANALOG15 129 +#define DIO129_BIT (ANALOG15_BIT) +#endif +#if (defined(DIN0_BIT)) +#define DIO130 130 +#define DIN0 130 +#define DIO130_BIT (DIN0_BIT) +#endif +#if (defined(DIN1_BIT)) +#define DIO131 131 +#define DIN1 131 +#define DIO131_BIT (DIN1_BIT) +#endif +#if (defined(DIN2_BIT)) +#define DIO132 132 +#define DIN2 132 +#define DIO132_BIT (DIN2_BIT) +#endif +#if (defined(DIN3_BIT)) +#define DIO133 133 +#define DIN3 133 +#define DIO133_BIT (DIN3_BIT) +#endif +#if (defined(DIN4_BIT)) +#define DIO134 134 +#define DIN4 134 +#define DIO134_BIT (DIN4_BIT) +#endif +#if (defined(DIN5_BIT)) +#define DIO135 135 +#define DIN5 135 +#define DIO135_BIT (DIN5_BIT) +#endif +#if (defined(DIN6_BIT)) +#define DIO136 136 +#define DIN6 136 +#define DIO136_BIT (DIN6_BIT) +#endif +#if (defined(DIN7_BIT)) +#define DIO137 137 +#define DIN7 137 +#define DIO137_BIT (DIN7_BIT) +#endif +#if (defined(DIN8_BIT)) +#define DIO138 138 +#define DIN8 138 +#define DIO138_BIT (DIN8_BIT) +#endif +#if (defined(DIN9_BIT)) +#define DIO139 139 +#define DIN9 139 +#define DIO139_BIT (DIN9_BIT) +#endif +#if (defined(DIN10_BIT)) +#define DIO140 140 +#define DIN10 140 +#define DIO140_BIT (DIN10_BIT) +#endif +#if (defined(DIN11_BIT)) +#define DIO141 141 +#define DIN11 141 +#define DIO141_BIT (DIN11_BIT) +#endif +#if (defined(DIN12_BIT)) +#define DIO142 142 +#define DIN12 142 +#define DIO142_BIT (DIN12_BIT) +#endif +#if (defined(DIN13_BIT)) +#define DIO143 143 +#define DIN13 143 +#define DIO143_BIT (DIN13_BIT) +#endif +#if (defined(DIN14_BIT)) +#define DIO144 144 +#define DIN14 144 +#define DIO144_BIT (DIN14_BIT) +#endif +#if (defined(DIN15_BIT)) +#define DIO145 145 +#define DIN15 145 +#define DIO145_BIT (DIN15_BIT) +#endif +#if (defined(DIN16_BIT)) +#define DIO146 146 +#define DIN16 146 +#define DIO146_BIT (DIN16_BIT) +#endif +#if (defined(DIN17_BIT)) +#define DIO147 147 +#define DIN17 147 +#define DIO147_BIT (DIN17_BIT) +#endif +#if (defined(DIN18_BIT)) +#define DIO148 148 +#define DIN18 148 +#define DIO148_BIT (DIN18_BIT) +#endif +#if (defined(DIN19_BIT)) +#define DIO149 149 +#define DIN19 149 +#define DIO149_BIT (DIN19_BIT) +#endif +#if (defined(DIN20_BIT)) +#define DIO150 150 +#define DIN20 150 +#define DIO150_BIT (DIN20_BIT) +#endif +#if (defined(DIN21_BIT)) +#define DIO151 151 +#define DIN21 151 +#define DIO151_BIT (DIN21_BIT) +#endif +#if (defined(DIN22_BIT)) +#define DIO152 152 +#define DIN22 152 +#define DIO152_BIT (DIN22_BIT) +#endif +#if (defined(DIN23_BIT)) +#define DIO153 153 +#define DIN23 153 +#define DIO153_BIT (DIN23_BIT) +#endif +#if (defined(DIN24_BIT)) +#define DIO154 154 +#define DIN24 154 +#define DIO154_BIT (DIN24_BIT) +#endif +#if (defined(DIN25_BIT)) +#define DIO155 155 +#define DIN25 155 +#define DIO155_BIT (DIN25_BIT) +#endif +#if (defined(DIN26_BIT)) +#define DIO156 156 +#define DIN26 156 +#define DIO156_BIT (DIN26_BIT) +#endif +#if (defined(DIN27_BIT)) +#define DIO157 157 +#define DIN27 157 +#define DIO157_BIT (DIN27_BIT) +#endif +#if (defined(DIN28_BIT)) +#define DIO158 158 +#define DIN28 158 +#define DIO158_BIT (DIN28_BIT) +#endif +#if (defined(DIN29_BIT)) +#define DIO159 159 +#define DIN29 159 +#define DIO159_BIT (DIN29_BIT) +#endif +#if (defined(DIN30_BIT)) +#define DIO160 160 +#define DIN30 160 +#define DIO160_BIT (DIN30_BIT) +#endif +#if (defined(DIN31_BIT)) +#define DIO161 161 +#define DIN31 161 +#define DIO161_BIT (DIN31_BIT) +#endif +#if(defined(DIN32_BIT)) +#define DIO162 162 +#define DIN32 162 +#define DIO162_BIT (DIN32_BIT) +#endif +#if(defined(DIN33_BIT)) +#define DIO163 163 +#define DIN33 163 +#define DIO163_BIT (DIN33_BIT) +#endif +#if(defined(DIN34_BIT)) +#define DIO164 164 +#define DIN34 164 +#define DIO164_BIT (DIN34_BIT) +#endif +#if(defined(DIN35_BIT)) +#define DIO165 165 +#define DIN35 165 +#define DIO165_BIT (DIN35_BIT) +#endif +#if(defined(DIN36_BIT)) +#define DIO166 166 +#define DIN36 166 +#define DIO166_BIT (DIN36_BIT) +#endif +#if(defined(DIN37_BIT)) +#define DIO167 167 +#define DIN37 167 +#define DIO167_BIT (DIN37_BIT) +#endif +#if(defined(DIN38_BIT)) +#define DIO168 168 +#define DIN38 168 +#define DIO168_BIT (DIN38_BIT) +#endif +#if(defined(DIN39_BIT)) +#define DIO169 169 +#define DIN39 169 +#define DIO169_BIT (DIN39_BIT) +#endif +#if(defined(DIN40_BIT)) +#define DIO170 170 +#define DIN40 170 +#define DIO170_BIT (DIN40_BIT) +#endif +#if(defined(DIN41_BIT)) +#define DIO171 171 +#define DIN41 171 +#define DIO171_BIT (DIN41_BIT) +#endif +#if(defined(DIN42_BIT)) +#define DIO172 172 +#define DIN42 172 +#define DIO172_BIT (DIN42_BIT) +#endif +#if(defined(DIN43_BIT)) +#define DIO173 173 +#define DIN43 173 +#define DIO173_BIT (DIN43_BIT) +#endif +#if(defined(DIN44_BIT)) +#define DIO174 174 +#define DIN44 174 +#define DIO174_BIT (DIN44_BIT) +#endif +#if(defined(DIN45_BIT)) +#define DIO175 175 +#define DIN45 175 +#define DIO175_BIT (DIN45_BIT) +#endif +#if(defined(DIN46_BIT)) +#define DIO176 176 +#define DIN46 176 +#define DIO176_BIT (DIN46_BIT) +#endif +#if(defined(DIN47_BIT)) +#define DIO177 177 +#define DIN47 177 +#define DIO177_BIT (DIN47_BIT) +#endif +#if(defined(DIN48_BIT)) +#define DIO178 178 +#define DIN48 178 +#define DIO178_BIT (DIN48_BIT) +#endif +#if(defined(DIN49_BIT)) +#define DIO179 179 +#define DIN49 179 +#define DIO179_BIT (DIN49_BIT) +#endif +#if (defined(TX_BIT)) +#define DIO200 200 +#define TX 200 +#define DIO200_BIT (TX_BIT) +#endif +#if (defined(RX_BIT)) +#define DIO201 201 +#define RX 201 +#define DIO201_BIT (RX_BIT) +#endif +#if (defined(USB_DM_BIT)) +#define DIO202 202 +#define USB_DM 202 +#define DIO202_BIT (USB_DM_BIT) +#endif +#if (defined(USB_DP_BIT)) +#define DIO203 203 +#define USB_DP 203 +#define DIO203_BIT (USB_DP_BIT) +#endif +#if (defined(SPI_CLK_BIT)) +#define DIO204 204 +#define SPI_CLK 204 +#define DIO204_BIT (SPI_CLK_BIT) +#endif +#if (defined(SPI_SDI_BIT)) +#define DIO205 205 +#define SPI_SDI 205 +#define DIO205_BIT (SPI_SDI_BIT) +#endif +#if (defined(SPI_SDO_BIT)) +#define DIO206 206 +#define SPI_SDO 206 +#define DIO206_BIT (SPI_SDO_BIT) +#endif +#if (defined(SPI_CS_BIT)) +#define DIO207 207 +#define SPI_CS 207 +#define DIO207_BIT (SPI_CS_BIT) +#endif +#if (defined(I2C_CLK_BIT)) +#define DIO208 208 +#define I2C_CLK 208 +#define DIO208_BIT (I2C_CLK_BIT) +#endif +#if (defined(I2C_DATA_BIT)) +#define DIO209 209 +#define I2C_DATA 209 +#define DIO209_BIT (I2C_DATA_BIT) +#endif +#if (defined(TX2_BIT)) +#define DIO210 210 +#define TX2 210 +#define DIO210_BIT (TX2_BIT) +#endif +#if (defined(RX2_BIT)) +#define DIO211 211 +#define RX2 211 +#define DIO211_BIT (RX2_BIT) +#endif +#if (defined(SPI2_CLK_BIT)) +#define DIO212 212 +#define SPI2_CLK 212 +#define DIO212_BIT (SPI2_CLK_BIT) +#endif +#if (defined(SPI2_SDI_BIT)) +#define DIO213 213 +#define SPI2_SDI 213 +#define DIO213_BIT (SPI2_SDI_BIT) +#endif +#if (defined(SPI2_SDO_BIT)) +#define DIO214 214 +#define SPI2_SDO 214 +#define DIO214_BIT (SPI2_SDO_BIT) +#endif +#if (defined(SPI2_CS_BIT)) +#define DIO215 215 +#define SPI2_CS 215 +#define DIO215_BIT (SPI2_CS_BIT) +#endif + +// ISR on change inputs +#if (defined(LIMIT_X_ISR) && defined(LIMIT_X)) +#define DIO52_ISR (LIMIT_X_ISR) +#define LIMIT_X_ISRCALLBACK mcu_limit_isr +#define DIO52_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_Y_ISR) && defined(LIMIT_Y)) +#define DIO53_ISR (LIMIT_Y_ISR) +#define LIMIT_Y_ISRCALLBACK mcu_limit_isr +#define DIO53_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_Z_ISR) && defined(LIMIT_Z)) +#define DIO54_ISR (LIMIT_Z_ISR) +#define LIMIT_Z_ISRCALLBACK mcu_limit_isr +#define DIO54_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_X2_ISR) && defined(LIMIT_X2)) +#define DIO55_ISR (LIMIT_X2_ISR) +#define LIMIT_X2_ISRCALLBACK mcu_limit_isr +#define DIO55_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_Y2_ISR) && defined(LIMIT_Y2)) +#define DIO56_ISR (LIMIT_Y2_ISR) +#define LIMIT_Y2_ISRCALLBACK mcu_limit_isr +#define DIO56_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_Z2_ISR) && defined(LIMIT_Z2)) +#define DIO57_ISR (LIMIT_Z2_ISR) +#define LIMIT_Z2_ISRCALLBACK mcu_limit_isr +#define DIO57_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_A_ISR) && defined(LIMIT_A)) +#define DIO58_ISR (LIMIT_A_ISR) +#define LIMIT_A_ISRCALLBACK mcu_limit_isr +#define DIO58_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_B_ISR) && defined(LIMIT_B)) +#define DIO59_ISR (LIMIT_B_ISR) +#define LIMIT_B_ISRCALLBACK mcu_limit_isr +#define DIO59_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(LIMIT_C_ISR) && defined(LIMIT_C)) +#define DIO60_ISR (LIMIT_C_ISR) +#define LIMIT_C_ISRCALLBACK mcu_limit_isr +#define DIO60_ISRCALLBACK mcu_limit_isr +#endif +#if (defined(PROBE_ISR) && defined(PROBE)) +#define DIO61_ISR (PROBE_ISR) +#define PROBE_ISRCALLBACK mcu_probe_isr +#define DIO61_ISRCALLBACK mcu_probe_isr +#endif +#if (defined(ESTOP_ISR) && defined(ESTOP)) +#define DIO62_ISR (ESTOP_ISR) +#define ESTOP_ISRCALLBACK mcu_control_isr +#define DIO62_ISRCALLBACK mcu_control_isr +#endif +#if (defined(SAFETY_DOOR_ISR) && defined(SAFETY_DOOR)) +#define DIO63_ISR (SAFETY_DOOR_ISR) +#define SAFETY_DOOR_ISRCALLBACK mcu_control_isr +#define DIO63_ISRCALLBACK mcu_control_isr +#endif +#if (defined(FHOLD_ISR) && defined(FHOLD)) +#define DIO64_ISR (FHOLD_ISR) +#define FHOLD_ISRCALLBACK mcu_control_isr +#define DIO64_ISRCALLBACK mcu_control_isr +#endif +#if (defined(CS_RES_ISR) && defined(CS_RES)) +#define DIO65_ISR (CS_RES_ISR) +#define CS_RES_ISRCALLBACK mcu_control_isr +#define DIO65_ISRCALLBACK mcu_control_isr +#endif +#if (defined(DIN0_ISR) && defined(DIN0)) +#define DIO82_ISR (DIN0_ISR) +#define DIN0_ISRCALLBACK mcu_din_isr +#define DIO82_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN1_ISR) && defined(DIN1)) +#define DIO83_ISR (DIN1_ISR) +#define DIN1_ISRCALLBACK mcu_din_isr +#define DIO83_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN2_ISR) && defined(DIN2)) +#define DIO84_ISR (DIN2_ISR) +#define DIN2_ISRCALLBACK mcu_din_isr +#define DIO84_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN3_ISR) && defined(DIN3)) +#define DIO85_ISR (DIN3_ISR) +#define DIN3_ISRCALLBACK mcu_din_isr +#define DIO85_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN4_ISR) && defined(DIN4)) +#define DIO86_ISR (DIN4_ISR) +#define DIN4_ISRCALLBACK mcu_din_isr +#define DIO86_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN5_ISR) && defined(DIN5)) +#define DIO87_ISR (DIN5_ISR) +#define DIN5_ISRCALLBACK mcu_din_isr +#define DIO87_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN6_ISR) && defined(DIN6)) +#define DIO88_ISR (DIN6_ISR) +#define DIN6_ISRCALLBACK mcu_din_isr +#define DIO88_ISRCALLBACK mcu_din_isr +#endif +#if (defined(DIN7_ISR) && defined(DIN7)) +#define DIO89_ISR (DIN7_ISR) +#define DIN7_ISRCALLBACK mcu_din_isr +#define DIO89_ISRCALLBACK mcu_din_isr +#endif + +// Helper macros +#define __helper_ex__(left, mid, right) (left##mid##right) +#define __helper__(left, mid, right) (__helper_ex__(left, mid, right)) +#ifndef __indirect__ +#define __indirect__ex__(X, Y) DIO##X##_##Y +#define __indirect__(X, Y) __indirect__ex__(X, Y) +#endif + +#if (defined(TX) && defined(RX)) +#define MCU_HAS_UART +#endif +#if (defined(TX2) && defined(RX2)) +#define MCU_HAS_UART2 +#endif +#if (defined(USB_DP) && defined(USB_DM)) +#define MCU_HAS_USB +#endif +// #ifdef ENABLE_WIFI +// #define MCU_HAS_WIFI +// #ifndef DISABLE_ENDPOINTS +// #define MCU_HAS_ENDPOINTS +// #endif +// #ifndef DISABLE_WEBSOCKETS +// #define MCU_HAS_WEBSOCKETS +// #endif +// #ifndef BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +// #define BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +// #endif +// #endif +// #ifdef ENABLE_BLUETOOTH +// #define MCU_HAS_BLUETOOTH +// #ifndef BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +// #define BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +// #endif +// #endif + +#ifdef MCU_HAS_UART +#ifndef UART_PORT +#define UART_PORT 0 +#endif +#if (UART_PORT == 0) +#define COM_UART Serial1 +#elif (UART_PORT == 1) +#define COM_UART Serial2 +#else +#error "UART COM port number must be 0 or 1" +#endif +#endif + +#ifdef MCU_HAS_UART2 +#ifndef BAUDRATE2 +#define BAUDRATE2 BAUDRATE +#endif +#ifndef UART2_PORT +#define UART2_PORT 0 +#endif +#if (UART2_PORT == 0) +#define COM2_UART Serial1 +#elif (UART2_PORT == 1) +#define COM2_UART Serial2 +#else +#error "UART2 COM port number must be 0 or 1" +#endif +#endif + +#define MCU_HAS_ONESHOT_TIMER + +// SPI +#if (defined(SPI_CLK) && defined(SPI_SDI) && defined(SPI_SDO)) +#define MCU_HAS_SPI +#ifndef SPI_MODE +#define SPI_MODE 0 +#endif +#ifndef SPI_FREQ +#define SPI_FREQ 1000000UL +#endif +#ifndef SPI_PORT +#define SPI_PORT 0 +#endif +#endif +#if (SPI_PORT == 0) +#define COM_SPI SPI +#elif (SPI_PORT == 1) +#define COM_SPI SPI1 +#else +#error "SPI port number must be 0 or 1" +#endif + +// SPI2 +#if (defined(SPI2_CLK) && defined(SPI2_SDI) && defined(SPI2_SDO)) +#define MCU_HAS_SPI2 +#ifndef SPI2_MODE +#define SPI2_MODE 0 +#endif +#ifndef SPI2_FREQ +#define SPI2_FREQ 1000000UL +#endif +#ifndef SPI2_PORT +#define SPI2_PORT 0 +#endif +#endif +#if (SPI2_PORT == 0) +#define COM_SPI2 SPI +#elif (SPI2_PORT == 1) +#define COM_SPI2 SPI1 +#else +#error "SPI2 port number must be 0 or 1" +#endif + +// for SDK SPI +#define SPI_HW __helper__(spi, SPI_PORT,) + +// for SDK SPI +#define SPI2_HW __helper__(spi, SPI2_PORT,) + + +#if (defined(I2C_CLK) && defined(I2C_DATA)) +#define MCU_HAS_I2C +#define MCU_SUPPORTS_I2C_SLAVE +#ifndef I2C_ADDRESS +#define I2C_ADDRESS 0 +#endif + +#ifndef I2C_PORT +#define I2C_PORT 0 +#endif +#ifndef I2C_FREQ +#define I2C_FREQ 400000UL +#endif +#endif + +#if (I2C_PORT == 0) +#define I2C_REG Wire +#elif (I2C_PORT == 1) +#define I2C_REG Wire1 +#else +#error "I2C port number must be 0 or 1" +#endif + +// since Arduino Pico does not allow access to pico-SDK functions low level timer functions are used +#ifndef ITP_TIMER +#define ITP_TIMER 1 +#endif + +// a single timer slot is used to do RTC, SERVO and ONESHOT +#ifndef RTC_TIMER +#define RTC_TIMER 2 +#endif + +#ifndef SERVO_TIMER +#undef SERVO_TIMER +#define SERVO_TIMER RTC_TIMER +#endif + +#ifndef ONESHOT_TIMER +#undef ONESHOT_TIMER +#define ONESHOT_TIMER RTC_TIMER +#define MCU_HAS_ONESHOT +#endif + +#define ALARM_TIMER RTC_TIMER +#define RTC_ALARM 0 +#define SERVO_ALARM 1 +#define ONESHOT_ALARM 2 + +#ifdef IC74HC595_CUSTOM_SHIFT_IO +#ifdef IC74HC595_COUNT +#undef IC74HC595_COUNT +#endif +// forces IC74HC595_COUNT to 4 to prevent errors +#define IC74HC595_COUNT 4 +#endif + +#define __timer_irq__(X) (TIMER0_IRQ_0 + X) +#define _timer_irq_(X) __timer_irq__(X) + +#define ITP_TIMER_IRQ _timer_irq_(ITP_TIMER) +#define RTC_TIMER_IRQ _timer_irq_(RTC_TIMER) +#define ALARM_TIMER_IRQ _timer_irq_(ALARM_TIMER) + +#ifndef BYTE_OPS +#define BYTE_OPS +#define SETBIT(x, y) ((x) |= (1UL << (y))) /* Set bit y in byte x*/ +#define CLEARBIT(x, y) ((x) &= ~(1UL << (y))) /* Clear bit y in byte x*/ +#define CHECKBIT(x, y) ((x) & (1UL << (y))) /* Check bit y in byte x*/ +#define TOGGLEBIT(x, y) ((x) ^= (1UL << (y))) /* Toggle bit y in byte x*/ + +#define SETFLAG(x, y) ((x) |= (y)) /* Set byte y in byte x*/ +#define CLEARFLAG(x, y) ((x) &= ~(y)) /* Clear byte y in byte x*/ +#define CHECKFLAG(x, y) ((x) & (y)) /* Check byte y in byte x*/ +#define TOGGLEFLAG(x, y) ((x) ^= (y)) /* Toggle byte y in byte x*/ +#endif + +#define mcu_config_output(X) pinMode(__indirect__(X, BIT), OUTPUT) +#define mcu_config_pwm(X, freq) \ + { \ + pinMode(__indirect__(X, BIT), OUTPUT); \ + analogWriteRange(255); \ + analogWriteFreq(freq); \ + analogWriteResolution(8); \ + } +#define mcu_config_input(X) pinMode(__indirect__(X, BIT), INPUT) +#define mcu_config_analog(X) mcu_config_input(X) +#define mcu_config_pullup(X) pinMode(__indirect__(X, BIT), INPUT_PULLUP) +#define mcu_config_input_isr(X) attachInterrupt(digitalPinToInterrupt(__indirect__(X, BIT)), mcu_din_isr, CHANGE) + +#define mcu_get_input(X) CHECKBIT(sio_hw->gpio_in, __indirect__(X, BIT)) +#define mcu_get_output(X) CHECKBIT(sio_hw->gpio_out, __indirect__(X, BIT)) +#define mcu_set_output(X) ({ sio_hw->gpio_set = (1UL << __indirect__(X, BIT)); }) +#define mcu_clear_output(X) ({ sio_hw->gpio_clr = (1UL << __indirect__(X, BIT)); }) +#define mcu_toggle_output(X) ({ sio_hw->gpio_togl = (1UL << __indirect__(X, BIT)); }) + + extern uint8_t rp2350_pwm[16]; +#define mcu_set_pwm(X, Y) \ + { \ + rp2350_pwm[X - PWM_PINS_OFFSET] = Y; \ + analogWrite(__indirect__(X, BIT), Y); \ + } +#define mcu_get_pwm(X) (rp2350_pwm[X - PWM_PINS_OFFSET]) +#define mcu_get_analog(X) analogRead(__indirect__(X, BIT)) + +#define mcu_millis() millis() +#define mcu_micros() micros() +#define mcu_free_micros() micros() + +#if (defined(ENABLE_WIFI) || defined(ENABLE_BLUETOOTH)) +#ifndef BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +#define BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +#endif +#endif + +/** + * Run code on multicore mode + * Launches code on core 0 + * Runs communications on core 0 + * Runs CNC loop on core 1 + * **/ +#ifdef RP2350_RUN_MULTICORE + +#define USE_CUSTOM_BUFFER_IMPLEMENTATION +#include +#define DECL_BUFFER(type, name, size) \ + static queue_t name##_bufferdata; \ + ring_buffer_t name = {0, 0, 0, (uint8_t *)&name##_bufferdata, size, sizeof(type)} +#define BUFFER_INIT(type, name, size) \ + extern ring_buffer_t name; \ + queue_init((queue_t *)name.data, sizeof(type), size) +#define BUFFER_WRITE_AVAILABLE(buffer) (buffer.size - queue_get_level((queue_t *)buffer.data)) +#define BUFFER_READ_AVAILABLE(buffer) (queue_get_level((queue_t *)buffer.data)) +#define BUFFER_EMPTY(buffer) queue_is_empty((queue_t *)buffer.data) +#define BUFFER_FULL(buffer) queue_is_full((queue_t *)buffer.data) +#define BUFFER_PEEK(buffer, ptr) \ + if (!queue_try_peek((queue_t *)buffer.data, ptr)) \ + { \ + memset(ptr, 0, buffer.elem_size); \ + } +#define BUFFER_DEQUEUE(buffer, ptr) \ + if (!queue_try_remove((queue_t *)buffer.data, ptr)) \ + { \ + memset(ptr, 0, buffer.elem_size); \ + } +#define BUFFER_ENQUEUE(buffer, ptr) queue_try_add((queue_t *)buffer.data, ptr) +#define BUFFER_WRITE(buffer, ptr, len, written) ({for(uint8_t i = 0; i + + µCNC is distributed WITHOUT ANY WARRANTY; + Also without the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + See the GNU General Public License for more details. +*/ + +#if defined(ARDUINO_ARCH_RP2040) && defined(TARGET_RP2350) +#include +#include +#include +#include +#include "../../../cnc.h" + +/** + * + * This handles all communications via Serial USB, Serial UART and WiFi + * + * **/ + +#if (defined(MCU_HAS_WIFI) || defined(ENABLE_BLUETOOTH)) + +#ifndef WIFI_SSID_MAX_LEN +#define WIFI_SSID_MAX_LEN 32 +#endif + +#ifndef WIFI_PASS_MAX_LEN +#define WIFI_PASS_MAX_LEN 32 +#endif + +#define ARG_MAX_LEN MAX(WIFI_SSID_MAX_LEN, WIFI_PASS_MAX_LEN) + +#ifdef ENABLE_BLUETOOTH +#include + +uint8_t bt_on; +uint16_t bt_settings_offset; +#endif + +#ifdef MCU_HAS_WIFI +#include +#include +#include + +#ifndef TELNET_PORT +#define TELNET_PORT 23 +#endif + +#ifndef WEBSERVER_PORT +#define WEBSERVER_PORT 80 +#endif + +#ifndef WEBSOCKET_PORT +#define WEBSOCKET_PORT 8080 +#endif + +#ifndef WEBSOCKET_MAX_CLIENTS +#define WEBSOCKET_MAX_CLIENTS 2 +#endif + +#ifndef WIFI_USER +#define WIFI_USER "admin\0" +#endif + +#ifndef WIFI_PASS +#define WIFI_PASS "pass\0" +#endif + +#ifndef OTA_URI +#define OTA_URI "/firmware" +#endif + +WebServer web_server(WEBSERVER_PORT); +HTTPUpdateServer httpUpdater; +const char *update_username = WIFI_USER; +const char *update_password = WIFI_PASS; +#define MAX_SRV_CLIENTS 1 +WiFiServer telnet_server(TELNET_PORT); +WiFiClient server_client; + +typedef struct +{ + uint8_t wifi_on; + uint8_t wifi_mode; + char ssid[WIFI_SSID_MAX_LEN]; + char pass[WIFI_PASS_MAX_LEN]; +} wifi_settings_t; + +uint16_t wifi_settings_offset; +wifi_settings_t wifi_settings; +#endif + +#ifdef BOARD_HAS_CUSTOM_SYSTEM_COMMANDS +bool mcu_custom_grbl_cmd(void *args) +{ + grbl_cmd_args_t *cmd_params = (grbl_cmd_args_t *)args; + uint8_t str[64]; + char arg[ARG_MAX_LEN]; + uint8_t has_arg = (cmd_params->next_char == '='); + memset(arg, 0, sizeof(arg)); + +#ifdef ENABLE_BLUETOOTH + if (!strncmp((const char *)(cmd_params->cmd), "BTH", 3)) + { + if (!strcmp((const char *)&(cmd_params->cmd)[3], "ON")) + { + SerialBT.begin(BAUDRATE, SERIAL_8N1); + proto_info("Bluetooth enabled"); + bt_on = 1; + settings_save(bt_settings_offset, &bt_on, 1); + + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[3], "OFF")) + { + SerialBT.end(); + proto_info("Bluetooth disabled"); + bt_on = 0; + settings_save(bt_settings_offset, &bt_on, 1); + + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + } +#endif +#ifdef MCU_HAS_WIFI + if (!strncmp((const char *)(cmd_params->cmd), "WIFI", 4)) + { + if (!strcmp((const char *)&(cmd_params->cmd)[4], "ON")) + { + WiFi.disconnect(); + switch (wifi_settings.wifi_mode) + { + case 1: + WiFi.mode(WIFI_STA); + WiFi.begin(wifi_settings.ssid, wifi_settings.pass); + proto_info("Trying to connect to WiFi"); + break; + case 2: + WiFi.mode(WIFI_AP); + WiFi.softAP(BOARD_NAME, wifi_settings.pass); + proto_info("AP started"); + proto_info("SSID>" BOARD_NAME); + proto_info("IP>%s", WiFi.softAPIP().toString().c_str()); + break; + default: + WiFi.mode(WIFI_AP_STA); + WiFi.begin(wifi_settings.ssid, wifi_settings.pass); + proto_info("Trying to connect to WiFi"); + WiFi.softAP(BOARD_NAME, wifi_settings.pass); + proto_info("AP started"); + proto_info("SSID>" BOARD_NAME); + proto_info("IP>%s", WiFi.softAPIP().toString().c_str()); + break; + } + + wifi_settings.wifi_on = 1; + settings_save(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "OFF")) + { + WiFi.disconnect(); + wifi_settings.wifi_on = 0; + settings_save(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "SSID")) + { + if (has_arg) + { + int8_t len = parser_get_grbl_cmd_arg(arg, ARG_MAX_LEN); + + if (len < 0) + { + *(cmd_params->error) = STATUS_INVALID_STATEMENT; + return EVENT_HANDLED; + } + if (len > WIFI_SSID_MAX_LEN) + { + proto_info("WiFi SSID is too long"); + } + memset(wifi_settings.ssid, 0, sizeof(wifi_settings.ssid)); + strcpy(wifi_settings.ssid, (const char *)arg); + settings_save(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + proto_info("WiFi SSID modified"); + } + else + { + proto_info("SSID>%s", wifi_settings.ssid); + } + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "SCAN")) + { + // Serial.println("[MSG:Scanning Networks]"); + proto_info("Scanning Networks"); + int numSsid = WiFi.scanNetworks(); + if (numSsid == -1) + { + proto_info("Failed to scan!"); + return EVENT_HANDLED; + } + + // print the list of networks seen: + proto_info("%d available networks", numSsid); + + // print the network number and name for each network found: + for (int netid = 0; netid < numSsid; netid++) + { + proto_info("%d) %s\tSignal: %ddBm", netid, WiFi.SSID(netid), WiFi.RSSI(netid)); + } + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "SAVE")) + { + settings_save(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + proto_info("WiFi settings saved"); + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "RESET")) + { + settings_erase(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + proto_info("WiFi settings deleted"); + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "MODE")) + { + if (has_arg) + { + int8_t len = parser_get_grbl_cmd_arg(arg, ARG_MAX_LEN); + + if (len < 0) + { + *(cmd_params->error) = STATUS_INVALID_STATEMENT; + return EVENT_HANDLED; + } + int mode = atoi((const char *)arg) - 1; + if (mode >= 0) + { + wifi_settings.wifi_mode = mode; + } + else + { + proto_info("Invalid value. STA+AP(1), STA(2), AP(3)"); + } + } + + switch (wifi_settings.wifi_mode) + { + case 0: + proto_info("WiFi mode>STA+AP"); + break; + case 1: + proto_info("WiFi mode>STA"); + break; + case 2: + proto_info("WiFi mode>AP"); + break; + } + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "PASS") && has_arg) + { + int8_t len = parser_get_grbl_cmd_arg(arg, ARG_MAX_LEN); + + if (len < 0) + { + *(cmd_params->error) = STATUS_INVALID_STATEMENT; + return EVENT_HANDLED; + } + if (len > WIFI_PASS_MAX_LEN) + { + proto_info("WiFi pass is too long"); + } + memset(wifi_settings.pass, 0, sizeof(wifi_settings.pass)); + strcpy(wifi_settings.pass, (const char *)arg); + proto_info("WiFi password modified"); + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + + if (!strcmp((const char *)&(cmd_params->cmd)[4], "IP")) + { + if (wifi_settings.wifi_on) + { + switch (wifi_settings.wifi_mode) + { + case 1: + proto_info("STA IP>%s", WiFi.localIP().toString().c_str()); + proto_info("AP IP>%s", WiFi.softAPIP().toString().c_str()); + break; + case 2: + proto_info("IP>%s", WiFi.localIP().toString().c_str()); + break; + default: + proto_info("IP>%s", WiFi.softAPIP().toString().c_str()); + break; + } + } + else + { + proto_info("WiFi is off"); + } + + *(cmd_params->error) = STATUS_OK; + return EVENT_HANDLED; + } + } +#endif + return EVENT_CONTINUE; +} + +CREATE_EVENT_LISTENER(grbl_cmd, mcu_custom_grbl_cmd); +#endif + +bool rp2350_wifi_clientok(void) +{ +#ifdef MCU_HAS_WIFI + static uint32_t next_info = 30000; + static bool connected = false; + uint8_t str[128]; + + if (!wifi_settings.wifi_on) + { + return false; + } + + if ((WiFi.status() != WL_CONNECTED)) + { + connected = false; + if (next_info > millis()) + { + return false; + } + next_info = millis() + 30000; + proto_info("Disconnected from WiFi"); + return false; + } + + if (!connected) + { + connected = true; + proto_info("Connected to WiFi"); + proto_info("SSID>%s", wifi_settings.ssid); + proto_info("IP>%s", WiFi.localIP().toString().c_str()); + } + + if (telnet_server.hasClient()) + { + if (server_client) + { + if (server_client.connected()) + { + server_client.stop(); + } + } + server_client = telnet_server.accept(); + server_client.println("[MSG:New client connected]"); + return false; + } + else if (server_client) + { + if (server_client.connected()) + { + return true; + } + } +#endif + return false; +} + +#if defined(MCU_HAS_WIFI) && defined(MCU_HAS_ENDPOINTS) + +#define MCU_FLASH_FS_LITTLE_FS 1 +#define MCU_FLASH_FS_SPIFFS 2 + +#ifndef MCU_FLASH_FS +#define MCU_FLASH_FS MCU_FLASH_FS_LITTLE_FS +#endif + +#if (MCU_FLASH_FS == MCU_FLASH_FS_LITTLE_FS) +#include "FS.h" +#include +#define FLASH_FS LittleFS +#elif (MCU_FLASH_FS == MCU_FLASH_FS_SPIFFS) +#include "FS.h" +#include +#define FLASH_FS SPIFFS +#endif + +/** + * Implements the function calls for the file system C wrapper + */ +#include "../../../modules/file_system.h" +#define fileptr_t(ptr) static_cast(*(reinterpret_cast(ptr))) +fs_t flash_fs; + +int flash_fs_available(fs_file_t *fp) +{ + return fileptr_t(fp->file_ptr).available(); +} + +void flash_fs_close(fs_file_t *fp) +{ + fileptr_t(fp->file_ptr).flush(); + fileptr_t(fp->file_ptr).close(); +} + +bool flash_fs_remove(const char *path) +{ + return FLASH_FS.remove(path); +} + +bool flash_fs_next_file(fs_file_t *fp, fs_file_info_t *finfo) +{ + File f = ((File *)fp->file_ptr)->openNextFile(); + if (!f || !finfo) + { + return false; + } + memset(finfo->full_name, 0, sizeof(finfo->full_name)); + strncpy(finfo->full_name, f.name(), (FS_PATH_NAME_MAX_LEN - strlen(f.name()))); + finfo->is_dir = f.isDirectory(); + finfo->size = f.size(); + finfo->timestamp = f.getLastWrite(); + f.close(); + return true; +} + +size_t flash_fs_read(fs_file_t *fp, uint8_t *buffer, size_t len) +{ + return fileptr_t(fp->file_ptr).read(buffer, len); +} + +size_t flash_fs_write(fs_file_t *fp, const uint8_t *buffer, size_t len) +{ + return fileptr_t(fp->file_ptr).write(buffer, len); +} + +bool flash_fs_info(const char *path, fs_file_info_t *finfo) +{ + File f = FLASH_FS.open(path, "r"); + if (f && finfo) + { + memset(finfo->full_name, 0, sizeof(finfo->full_name)); + strncpy(finfo->full_name, f.name(), (FS_PATH_NAME_MAX_LEN - strlen(f.name()))); + finfo->is_dir = f.isDirectory(); + finfo->size = f.size(); + finfo->timestamp = (uint32_t)f.getLastWrite(); + f.close(); + return true; + } + + return false; +} + +fs_file_t *flash_fs_open(const char *path, const char *mode) +{ + fs_file_t *fp = (fs_file_t *)calloc(1, sizeof(fs_file_t)); + if (fp) + { + fp->file_ptr = calloc(1, sizeof(File)); + if (fp->file_ptr) + { + *(static_cast(fp->file_ptr)) = FLASH_FS.open(path, mode); + if (*(static_cast(fp->file_ptr))) + { + memset(fp->file_info.full_name, 0, sizeof(fp->file_info.full_name)); + fp->file_info.full_name[0] = '/'; + fp->file_info.full_name[1] = flash_fs.drive; + fp->file_info.full_name[2] = '/'; + strncat(fp->file_info.full_name, ((File *)fp->file_ptr)->name(), FS_PATH_NAME_MAX_LEN - 3); + fp->file_info.is_dir = ((File *)fp->file_ptr)->isDirectory(); + fp->file_info.size = ((File *)fp->file_ptr)->size(); + fp->file_info.timestamp = (uint32_t)((File *)fp->file_ptr)->getLastWrite(); + fp->fs_ptr = &flash_fs; + return fp; + } + fs_safe_free(fp->file_ptr); + } + fs_safe_free(fp); + } + return NULL; +} + +fs_file_t *flash_fs_opendir(const char *path) +{ + return flash_fs_open(path, "r"); +} + +bool flash_fs_seek(fs_file_t *fp, uint32_t position) +{ + return fp->fs_ptr->seek(fp, position); +} + +bool flash_fs_mkdir(const char *path) +{ + return FLASH_FS.mkdir(path); +} + +bool flash_fs_rmdir(const char *path) +{ + return FLASH_FS.rmdir(path); +} + +/** + * Implements the function calls for the enpoints C wrapper + */ +#include "../../../modules/endpoint.h" +void endpoint_add(const char *uri, uint8_t method, endpoint_delegate request_handler, endpoint_delegate file_handler) +{ + if (!method) + { + method = HTTP_ANY; + } + + String s = String(uri); + + if (s.endsWith("*")) + { + web_server.on(UriWildcard(s.substring(0, s.length() - 1)), (HTTPMethod)method, request_handler, file_handler); + } + else + { + web_server.on(Uri(uri), (HTTPMethod)method, request_handler, file_handler); + } +} + +void endpoint_request_uri(char *uri, size_t maxlen) +{ + strncpy(uri, web_server.uri().c_str(), maxlen); +} + +int endpoint_request_hasargs(void) +{ + return web_server.args(); +} + +bool endpoint_request_arg(const char *argname, char *argvalue, size_t maxlen) +{ + if (!web_server.hasArg(String(argname))) + { + argvalue[0] = 0; + return false; + } + strncpy(argvalue, web_server.arg(String(argname)).c_str(), maxlen); + return true; +} + +void endpoint_send(int code, const char *content_type, const uint8_t *data, size_t data_len) +{ + static uint8_t in_chuncks = 0; + if (!content_type) + { + in_chuncks = 1; + web_server.setContentLength(CONTENT_LENGTH_UNKNOWN); + } + else + { + switch (in_chuncks) + { + case 1: + in_chuncks = 2; + __FALL_THROUGH__ + case 0: + web_server.send(code, content_type, data, data_len); + break; + default: + if (data) + { + web_server.sendContent((char *)data, data_len); + in_chuncks = 2; + } + else + { + web_server.sendContent(""); + in_chuncks = 0; + } + break; + } + } +} + +void endpoint_send_header(const char *name, const char *data, bool first) +{ + web_server.sendHeader(name, data, first); +} + +bool endpoint_send_file(const char *file_path, const char *content_type) +{ + if (FLASH_FS.exists(file_path)) + { + File file = FLASH_FS.open(file_path, "r"); + web_server.streamFile(file, content_type); + file.close(); + return true; + } + return false; +} + +endpoint_upload_t endpoint_file_upload_status(void) +{ + HTTPUpload &upload = web_server.upload(); + endpoint_upload_t status = {.status = (uint8_t)upload.status, .data = upload.buf, .datalen = upload.currentSize}; + return status; +} + +uint8_t endpoint_request_method(void) +{ + switch (web_server.method()) + { + case HTTP_GET: + return ENDPOINT_GET; + case HTTP_POST: + return ENDPOINT_POST; + case HTTP_PUT: + return ENDPOINT_PUT; + case HTTP_DELETE: + return ENDPOINT_DELETE; + default: + return (ENDPOINT_OTHER | (uint8_t)web_server.method()); + } +} + +void endpoint_file_upload_name(char *filename, size_t maxlen) +{ + HTTPUpload &upload = web_server.upload(); + strncat(filename, upload.filename.c_str(), maxlen - strlen(filename)); +} + +#endif + +#if defined(MCU_HAS_WIFI) && defined(MCU_HAS_WEBSOCKETS) +#include "WebSocketsServer.h" +#include "../../../modules/websocket.h" +WebSocketsServer socket_server(WEBSOCKET_PORT); + +WEAK_EVENT_HANDLER(websocket_client_connected) +{ + DEFAULT_EVENT_HANDLER(websocket_client_connected); +} + +WEAK_EVENT_HANDLER(websocket_client_disconnected) +{ + DEFAULT_EVENT_HANDLER(websocket_client_disconnected); +} + +WEAK_EVENT_HANDLER(websocket_client_receive) +{ + DEFAULT_EVENT_HANDLER(websocket_client_receive); +} + +WEAK_EVENT_HANDLER(websocket_client_error) +{ + DEFAULT_EVENT_HANDLER(websocket_client_error); +} + +void websocket_send(uint8_t clientid, uint8_t *data, size_t length, uint8_t flags) +{ + switch (flags & WS_SEND_TYPE) + { + case WS_SEND_TXT: + if (flags & WS_SEND_BROADCAST) + { + socket_server.broadcastTXT(data, length); + } + else + { + socket_server.sendTXT(clientid, data, length); + } + break; + case WS_SEND_BIN: + if (flags & WS_SEND_BROADCAST) + { + socket_server.broadcastTXT(data, length); + } + else + { + socket_server.sendTXT(clientid, data, length); + } + break; + case WS_SEND_PING: + if (flags & WS_SEND_BROADCAST) + { + socket_server.broadcastPing(data, length); + } + else + { + socket_server.sendPing(clientid, data, length); + } + break; + } +} + +void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) +{ + websocket_event_t event = {num, (uint32_t)socket_server.remoteIP(num), type, payload, length}; + switch (type) + { + case WStype_DISCONNECTED: + EVENT_INVOKE(websocket_client_disconnected, &event); + break; + case WStype_CONNECTED: + EVENT_INVOKE(websocket_client_connected, &event); + break; + case WStype_ERROR: + EVENT_INVOKE(websocket_client_error, &event); + break; + case WStype_TEXT: + case WStype_BIN: + case WStype_FRAGMENT_TEXT_START: + case WStype_FRAGMENT_BIN_START: + case WStype_FRAGMENT: + case WStype_FRAGMENT_FIN: + case WStype_PING: + case WStype_PONG: + EVENT_INVOKE(websocket_client_receive, &event); + break; + } +} +#endif + +void rp2350_wifi_bt_init(void) +{ +#ifdef MCU_HAS_WIFI + + wifi_settings_offset = settings_register_external_setting(sizeof(wifi_settings_t)); + if (settings_load(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t))) + { + wifi_settings = {0}; + memcpy(wifi_settings.ssid, BOARD_NAME, strlen((const char *)BOARD_NAME)); + memcpy(wifi_settings.pass, WIFI_PASS, strlen((const char *)WIFI_PASS)); + settings_save(wifi_settings_offset, (uint8_t *)&wifi_settings, sizeof(wifi_settings_t)); + } + + if (wifi_settings.wifi_on) + { + uint8_t str[64]; + + switch (wifi_settings.wifi_mode) + { + case 1: + WiFi.mode(WIFI_STA); + WiFi.begin((char *)wifi_settings.ssid, (char *)wifi_settings.pass); + proto_info("Trying to connect to WiFi"); + break; + case 2: + WiFi.mode(WIFI_AP); + WiFi.softAP(BOARD_NAME, (char *)wifi_settings.pass); + proto_info("AP started"); + proto_info("SSID>" BOARD_NAME); + proto_info("IP>%s", WiFi.softAPIP().toString().c_str()); + break; + default: + WiFi.mode(WIFI_AP_STA); + WiFi.begin((char *)wifi_settings.ssid, (char *)wifi_settings.pass); + proto_info("Trying to connect to WiFi"); + WiFi.softAP(BOARD_NAME, (char *)wifi_settings.pass); + proto_info("AP started"); + proto_info("SSID>" BOARD_NAME); + proto_info("IP>%s", WiFi.softAPIP().toString().c_str()); + break; + } + } + telnet_server.begin(); + telnet_server.setNoDelay(true); +#ifdef MCU_HAS_ENDPOINTS + FLASH_FS.begin(); + flash_fs = { + .drive = 'C', + .open = flash_fs_open, + .read = flash_fs_read, + .write = flash_fs_write, + .seek = flash_fs_seek, + .available = flash_fs_available, + .close = flash_fs_close, + .remove = flash_fs_remove, + .opendir = flash_fs_opendir, + .mkdir = flash_fs_mkdir, + .rmdir = flash_fs_rmdir, + .next_file = flash_fs_next_file, + .finfo = flash_fs_info, + .next = NULL}; + fs_mount(&flash_fs); +#endif +#ifndef CUSTOM_OTA_ENDPOINT + httpUpdater.setup(&web_server, OTA_URI, update_username, update_password); +#endif + web_server.begin(); + +#ifdef MCU_HAS_WEBSOCKETS + socket_server.begin(); + socket_server.onEvent(webSocketEvent); +#endif +#endif +#ifdef ENABLE_BLUETOOTH + bt_settings_offset = settings_register_external_setting(1); + if (settings_load(bt_settings_offset, &bt_on, 1)) + { + settings_erase(bt_settings_offset, (uint8_t *)&bt_on, 1); + } + + if (bt_on) + { + SerialBT.begin(BAUDRATE, SERIAL_8N1); + } +#endif + +#ifdef BOARD_HAS_CUSTOM_SYSTEM_COMMANDS + ADD_EVENT_LISTENER(grbl_cmd, mcu_custom_grbl_cmd); +#endif +} + +#ifdef MCU_HAS_WIFI +#ifndef WIFI_TX_BUFFER_SIZE +#define WIFI_TX_BUFFER_SIZE 64 +#endif +DECL_BUFFER(uint8_t, wifi_tx, WIFI_TX_BUFFER_SIZE); +DECL_BUFFER(uint8_t, wifi_rx, RX_BUFFER_SIZE); + +uint8_t mcu_wifi_getc(void) +{ + uint8_t c = 0; + BUFFER_DEQUEUE(wifi_rx, &c); + return c; +} + +uint8_t mcu_wifi_available(void) +{ + return BUFFER_READ_AVAILABLE(wifi_rx); +} + +void mcu_wifi_clear(void) +{ + BUFFER_CLEAR(wifi_rx); +} + +void mcu_wifi_putc(uint8_t c) +{ + while (BUFFER_FULL(wifi_tx)) + { + mcu_wifi_flush(); + } + BUFFER_ENQUEUE(wifi_tx, &c); +} + +void mcu_wifi_flush(void) +{ + if (rp2350_wifi_clientok()) + { + while (!BUFFER_EMPTY(wifi_tx)) + { + uint8_t tmp[WIFI_TX_BUFFER_SIZE + 1]; + memset(tmp, 0, sizeof(tmp)); + uint8_t r; + + BUFFER_READ(wifi_tx, tmp, WIFI_TX_BUFFER_SIZE, r); + server_client.write(tmp, r); + } + } + else + { + // no client (discard) + BUFFER_CLEAR(wifi_tx); + } +} +#endif + +#ifdef MCU_HAS_BLUETOOTH +#ifndef BLUETOOTH_TX_BUFFER_SIZE +#define BLUETOOTH_TX_BUFFER_SIZE 64 +#endif +DECL_BUFFER(uint8_t, bt_tx, BLUETOOTH_TX_BUFFER_SIZE); +DECL_BUFFER(uint8_t, bt_rx, RX_BUFFER_SIZE); + +uint8_t mcu_bt_getc(void) +{ + uint8_t c = 0; + BUFFER_DEQUEUE(bt_rx, &c); + return c; +} + +uint8_t mcu_bt_available(void) +{ + return BUFFER_READ_AVAILABLE(bt_rx); +} + +void mcu_bt_clear(void) +{ + BUFFER_CLEAR(bt_rx); +} + +void mcu_bt_putc(uint8_t c) +{ + while (BUFFER_FULL(bt_tx)) + { + mcu_bt_flush(); + } + BUFFER_ENQUEUE(bt_tx, &c); +} + +void mcu_bt_flush(void) +{ + while (!BUFFER_EMPTY(bt_tx)) + { + uint8_t tmp[BLUETOOTH_TX_BUFFER_SIZE + 1]; + memset(tmp, 0, sizeof(tmp)); + uint8_t r; + + BUFFER_READ(bt_tx, tmp, BLUETOOTH_TX_BUFFER_SIZE, r); + SerialBT.write(tmp, r); + SerialBT.flush(); + } +} +#endif + +uint8_t rp2350_wifi_bt_read(void) +{ +#ifdef MCU_HAS_WIFI + if (rp2350_wifi_clientok()) + { + if (server_client.available() > 0) + { + return (uint8_t)server_client.read(); + } + } +#endif + +#ifdef ENABLE_BLUETOOTH + return (uint8_t)SerialBT.read(); +#endif + + return (uint8_t)0; +} + +void rp2350_wifi_bt_process(void) +{ +#ifdef MCU_HAS_WIFI + if (rp2350_wifi_clientok()) + { + while (server_client.available() > 0) + { +#ifndef DETACH_WIFI_FROM_MAIN_PROTOCOL + uint8_t c = (uint8_t)server_client.read(); + if (mcu_com_rx_cb(c)) + { + if (BUFFER_FULL(wifi_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(wifi_rx, &c); + } + +#else + mcu_wifi_rx_cb((uint8_t)server_client.read()); +#endif + } + } + + if (wifi_settings.wifi_on) + { + web_server.handleClient(); +#ifdef MCU_HAS_WEBSOCKETS + socket_server.loop(); +#endif + } +#endif + +#ifdef ENABLE_BLUETOOTH + while (SerialBT.available() > 0) + { +#ifndef DETACH_BLUETOOTH_FROM_MAIN_PROTOCOL + uint8_t c = (uint8_t)SerialBT.read(); + if (mcu_com_rx_cb(c)) + { + if (BUFFER_FULL(bt_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(bt_rx, &c); + } + +#else + mcu_bt_rx_cb((uint8_t)SerialBT.read()); +#endif + } +#endif +} + +#endif + +/** + * + * This handles EEPROM simulation on flash memory + * + * **/ + +#ifndef RAM_ONLY_SETTINGS +#include +extern "C" +{ + void rp2350_eeprom_init(int size) + { + EEPROM.begin(size); + } + + uint8_t rp2350_eeprom_read(uint16_t address) + { + return EEPROM.read(address); + } + + void rp2350_eeprom_write(uint16_t address, uint8_t value) + { + EEPROM.write(address, value); + } + + void rp2350_eeprom_flush(void) + { +#ifndef RP2350_RUN_MULTICORE + if (!EEPROM.commit()) + { + proto_info(" EEPROM write error"); + } +#else + // signal other core to store EEPROM + rp2350.fifo.push(0); + // wait for signal back + rp2350.fifo.pop(); +#endif + } +} +#endif + +extern "C" +{ + void rp2350_uart_init(int baud) + { +#ifdef MCU_HAS_USB + Serial.begin(baud); +#endif +#ifdef MCU_HAS_UART + COM_UART.setTX(TX_BIT); + COM_UART.setRX(RX_BIT); + COM_UART.begin(BAUDRATE); +#endif +#ifdef MCU_HAS_UART2 + COM2_UART.setTX(TX2_BIT); + COM2_UART.setRX(RX2_BIT); + COM2_UART.begin(BAUDRATE2); +#endif +#if (defined(MCU_HAS_WIFI) || defined(ENABLE_BLUETOOTH)) + rp2350_wifi_bt_init(); +#endif + } + +#ifdef MCU_HAS_USB +#ifndef USB_TX_BUFFER_SIZE +#define USB_TX_BUFFER_SIZE 64 +#endif + DECL_BUFFER(uint8_t, usb_tx, USB_TX_BUFFER_SIZE); + DECL_BUFFER(uint8_t, usb_rx, RX_BUFFER_SIZE); + + uint8_t mcu_usb_getc(void) + { + uint8_t c = 0; + BUFFER_DEQUEUE(usb_rx, &c); + return c; + } + + uint8_t mcu_usb_available(void) + { + return BUFFER_READ_AVAILABLE(usb_rx); + } + + void mcu_usb_clear(void) + { + BUFFER_CLEAR(usb_rx); + } + + void mcu_usb_putc(uint8_t c) + { + while (BUFFER_FULL(usb_tx)) + { + mcu_usb_flush(); + } + BUFFER_ENQUEUE(usb_tx, &c); + } + + void mcu_usb_flush(void) + { + while (!BUFFER_EMPTY(usb_tx)) + { + uint8_t tmp[USB_TX_BUFFER_SIZE + 1]; + memset(tmp, 0, sizeof(tmp)); + uint8_t r; + + BUFFER_READ(usb_tx, tmp, USB_TX_BUFFER_SIZE, r); + Serial.write(tmp, r); + Serial.flush(); + } + } +#endif + +#ifdef MCU_HAS_UART +#ifndef UART_TX_BUFFER_SIZE +#define UART_TX_BUFFER_SIZE 64 +#endif + DECL_BUFFER(uint8_t, uart_tx, UART_TX_BUFFER_SIZE); + DECL_BUFFER(uint8_t, uart_rx, RX_BUFFER_SIZE); + + uint8_t mcu_uart_getc(void) + { + uint8_t c = 0; + BUFFER_DEQUEUE(uart_rx, &c); + return c; + } + + uint8_t mcu_uart_available(void) + { + return BUFFER_READ_AVAILABLE(uart_rx); + } + + void mcu_uart_clear(void) + { + BUFFER_CLEAR(uart_rx); + } + + void mcu_uart_putc(uint8_t c) + { + while (BUFFER_FULL(uart_tx)) + { + mcu_uart_flush(); + } + BUFFER_ENQUEUE(uart_tx, &c); + } + + void mcu_uart_flush(void) + { + while (!BUFFER_EMPTY(uart_tx)) + { + uint8_t tmp[UART_TX_BUFFER_SIZE + 1]; + memset(tmp, 0, sizeof(tmp)); + uint8_t r = 0; + + BUFFER_READ(uart_tx, tmp, UART_TX_BUFFER_SIZE, r); + COM_UART.write(tmp, r); + COM_UART.flush(); + } + } +#endif + +#ifdef MCU_HAS_UART2 +#ifndef UART2_TX_BUFFER_SIZE +#define UART2_TX_BUFFER_SIZE 64 +#endif + DECL_BUFFER(uint8_t, uart2_tx, UART2_TX_BUFFER_SIZE); + DECL_BUFFER(uint8_t, uart2_rx, RX_BUFFER_SIZE); + + uint8_t mcu_uart2_getc(void) + { + uint8_t c = 0; + BUFFER_DEQUEUE(uart2_rx, &c); + return c; + } + + uint8_t mcu_uart2_available(void) + { + return BUFFER_READ_AVAILABLE(uart2_rx); + } + + void mcu_uart2_clear(void) + { + BUFFER_CLEAR(uart2_rx); + } + + void mcu_uart2_putc(uint8_t c) + { + while (BUFFER_FULL(uart2_tx)) + { + mcu_uart2_flush(); + } + BUFFER_ENQUEUE(uart2_tx, &c); + } + + void mcu_uart2_flush(void) + { + while (!BUFFER_EMPTY(uart2_tx)) + { + uint8_t tmp[UART2_TX_BUFFER_SIZE + 1]; + memset(tmp, 0, sizeof(tmp)); + uint8_t r; + + BUFFER_READ(uart2_tx, tmp, UART2_TX_BUFFER_SIZE, r); + COM2_UART.write(tmp, r); + COM2_UART.flush(); + } + } +#endif + + void rp2350_uart_process(void) + { +#ifdef MCU_HAS_USB + while (Serial.available() > 0) + { +#ifndef DETACH_USB_FROM_MAIN_PROTOCOL + uint8_t c = (uint8_t)Serial.read(); + if (mcu_com_rx_cb(c)) + { + if (BUFFER_FULL(usb_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(usb_rx, &c); + } + +#else + mcu_usb_rx_cb((uint8_t)Serial.read()); +#endif + } +#endif + +#ifdef MCU_HAS_UART + while (COM_UART.available() > 0) + { +#ifndef DETACH_UART_FROM_MAIN_PROTOCOL + uint8_t c = (uint8_t)COM_UART.read(); + if (mcu_com_rx_cb(c)) + { + if (BUFFER_FULL(uart_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(uart_rx, &c); + } +#else + mcu_uart_rx_cb((uint8_t)COM_UART.read()); +#endif + } +#endif + +#ifdef MCU_HAS_UART2 + while (COM2_UART.available() > 0) + { + uint8_t c = (uint8_t)COM2_UART.read(); +#ifndef DETACH_UART2_FROM_MAIN_PROTOCOL + + if (mcu_com_rx_cb(c)) + { + if (BUFFER_FULL(uart2_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(uart2_rx, &c); + } + +#else + mcu_uart2_rx_cb(c); +#ifndef UART2_DISABLE_BUFFER + if (BUFFER_FULL(uart2_rx)) + { + c = OVF; + } + + BUFFER_ENQUEUE(uart2_rx, &c); +#endif +#endif + } +#endif + +#if (defined(MCU_HAS_WIFI) || defined(ENABLE_BLUETOOTH)) + rp2350_wifi_bt_process(); +#endif + +#if defined(RP2350_RUN_MULTICORE) && !defined(RAM_ONLY_SETTINGS) + // flush pending eeprom request + if (rp2350.fifo.available()) + { + rp2350.fifo.pop(); + if (!EEPROM.commit()) + { + proto_info(" EEPROM write error"); + } + rp2350.fifo.push(0); + } +#endif + } +} + +/** + * + * This handles SPI communications + * + * **/ + +#if defined(MCU_HAS_SPI) && defined(USE_ARDUINO_SPI_LIBRARY) +#include +extern "C" +{ + void mcu_spi_config(spi_config_t config, uint32_t frequency) + { + COM_SPI.end(); + COM_SPI.setRX(SPI_SDI_BIT); + COM_SPI.setTX(SPI_SDO_BIT); + COM_SPI.setSCK(SPI_CLK_BIT); + COM_SPI.setCS(SPI_CS_BIT); + COM_SPI.begin(); + } + + uint8_t mcu_spi_xmit(uint8_t data) + { + return COM_SPI.transfer(data); + } + + void mcu_spi_start(spi_config_t config, uint32_t frequency) + { + COM_SPI.beginTransaction(SPISettings(frequency, MSBFIRST, config.mode)); + } + + void mcu_spi_stop(void) + { + COM_SPI.endTransaction(); + } + + bool mcu_spi_bulk_transfer(const uint8_t *out, uint8_t *in, uint16_t len) + { + COM_SPI.transfer((const void *)out, (void *)in, len); + return false; + } +} + +#endif + +#if defined(MCU_HAS_SPI2) && defined(USE_ARDUINO_SPI_LIBRARY) +#include +extern "C" +{ + void mcu_spi2_config(spi_config_t config, uint32_t frequency) + { + COM_SPI2.end(); + COM_SPI2.setRX(SPI2_SDI_BIT); + COM_SPI2.setTX(SPI2_SDO_BIT); + COM_SPI2.setSCK(SPI2_CLK_BIT); + COM_SPI2.setCS(SPI2_CS_BIT); + COM_SPI2.begin(); + } + + uint8_t mcu_spi2_xmit(uint8_t data) + { + return COM_SPI2.transfer(data); + } + + void mcu_spi2_start(spi_config_t config, uint32_t frequency) + { + COM_SPI2.beginTransaction(SPISettings(frequency, MSBFIRST, config.mode)); + } + + void mcu_spi2_stop(void) + { + COM_SPI2.endTransaction(); + } + + bool mcu_spi2_bulk_transfer(const uint8_t *out, uint8_t *in, uint16_t len) + { + COM_SPI2.transfer((const void *)out, (void *)in, len); + return false; + } +} + +#endif + +/** + * + * This handles I2C communications + * + * **/ + +#ifdef MCU_HAS_I2C +#include +extern "C" +{ +#if (I2C_ADDRESS != 0) + static uint8_t mcu_i2c_buffer_len; + static uint8_t mcu_i2c_buffer[I2C_SLAVE_BUFFER_SIZE]; + void rp2350_i2c_onreceive(int len) + { + uint8_t l = I2C_REG.readBytes(mcu_i2c_buffer, len); + mcu_i2c_slave_cb(mcu_i2c_buffer, &l); + mcu_i2c_buffer_len = l; + } + + void rp2350_i2c_onrequest(void) + { + I2C_REG.write(mcu_i2c_buffer, mcu_i2c_buffer_len); + } + +#endif + + void mcu_i2c_config(uint32_t frequency) + { + I2C_REG.setSDA(I2C_DATA_BIT); + I2C_REG.setSCL(I2C_CLK_BIT); +#if I2C_ADDRESS == 0 + I2C_REG.setClock(frequency); + I2C_REG.begin(); +#else + I2C_REG.onReceive(rp2350_i2c_onreceive); + I2C_REG.onRequest(rp2350_i2c_onrequest); + I2C_REG.begin(I2C_ADDRESS); +#endif + } + + uint8_t mcu_i2c_send(uint8_t address, uint8_t *data, uint8_t datalen, bool release, uint32_t ms_timeout) + { + I2C_REG.beginTransmission(address); + I2C_REG.write(data, datalen); + return (I2C_REG.endTransmission(release) == 0) ? I2C_OK : I2C_NOTOK; + } + + uint8_t mcu_i2c_receive(uint8_t address, uint8_t *data, uint8_t datalen, uint32_t ms_timeout) + { + if (I2C_REG.requestFrom(address, datalen) == datalen) + { + I2C_REG.readBytes(data, datalen); + return I2C_OK; + } + + return I2C_NOTOK; + } +} +#endif + +#endif