Ny structure. Start of the "main" branch

Cards/EaserCAT-2000/Firmware/.gitignore

@@ -0,0 +1,6 @@
+.pio
+.vscode/.browse.c_cpp.db*
+.vscode/c_cpp_properties.json
+.vscode/launch.json
+.vscode/ipch
+.vscode/settings.json

Cards/EaserCAT-2000/Firmware/.vscode/extensions.json

@@ -0,0 +1,10 @@
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

Cards/EaserCAT-2000/Firmware/doc/.gitignore

@@ -0,0 +1 @@
+.~lock.*

Cards/EaserCAT-2000/Firmware/include/MyEncoder.h

@@ -0,0 +1,38 @@
+#ifndef MYENCODER
+#define MYENCODER
+#include "Stm32F4_Encoder.h"
+#include <CircularBuffer.hpp>
+#define RINGBUFFERLEN 11
+
+class MyEncoder
+{
+public:
+    MyEncoder(TIM_TypeDef *_tim_base, uint8_t _indexPin, void irq(void));
+    int64_t unwrapEncoder(uint16_t in);
+    void indexPulse(void);
+    uint8_t indexHappened();
+    double currentPos();
+    double frequency(uint64_t time);
+    uint8_t getIndexState();
+    void setScale(double scale);
+    void setLatch(uint8_t latchEnable);
+
+private:
+    int64_t previousEncoderCounterValue = 0;
+    double PosScaleRes = 1.0;
+    uint32_t CurPosScale = 1;
+    uint8_t oldLatchCEnable = 0;
+    volatile uint8_t indexPulseFired = 0;
+    volatile uint8_t pleaseZeroTheCounter = 0;
+    Encoder EncoderInit;
+    uint8_t indexPin;
+
+    CircularBuffer<double_t, RINGBUFFERLEN> Pos;
+    CircularBuffer<uint32_t, RINGBUFFERLEN> TDelta;
+    double curPos;
+    double oldFrequency;
+
+    TIM_TypeDef *tim_base;
+};
+
+#endif

Cards/EaserCAT-2000/Firmware/include/README

@@ -0,0 +1,39 @@
+
+This directory is intended for project header files.
+
+A header file is a file containing C declarations and macro definitions
+to be shared between several project source files. You request the use of a
+header file in your project source file (C, C++, etc) located in `src` folder
+by including it, with the C preprocessing directive `#include'.
+
+```src/main.c
+
+#include "header.h"
+
+int main (void)
+{
+ ...
+}
+```
+
+Including a header file produces the same results as copying the header file
+into each source file that needs it. Such copying would be time-consuming
+and error-prone. With a header file, the related declarations appear
+in only one place. If they need to be changed, they can be changed in one
+place, and programs that include the header file will automatically use the
+new version when next recompiled. The header file eliminates the labor of
+finding and changing all the copies as well as the risk that a failure to
+find one copy will result in inconsistencies within a program.
+
+In C, the usual convention is to give header files names that end with `.h'.
+It is most portable to use only letters, digits, dashes, and underscores in
+header file names, and at most one dot.
+
+Read more about using header files in official GCC documentation:
+
+* Include Syntax
+* Include Operation
+* Once-Only Headers
+* Computed Includes
+
+https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

Cards/EaserCAT-2000/Firmware/include/StepGen.h

@@ -0,0 +1,43 @@
+
+#ifndef STEPGEN
+#define STEPGEN
+#include <HardwareTimer.h>
+
+class StepGen
+{
+private:
+    volatile uint8_t timerIsRunning;
+    volatile int32_t timerStepPosition;
+    volatile int32_t timerStepDirection;
+    volatile int32_t timerStepPositionAtEnd;
+    volatile int32_t timerNewEndStepPosition;
+    volatile uint32_t timerNewCycleTime;
+    volatile double_t actualPosition;
+    volatile double_t requestedPosition;
+    volatile uint8_t enabled;
+    HardwareTimer *MyTim;
+    HardwareTimer *MyTim2;
+    uint16_t stepsPerMM;
+    uint8_t dirPin;
+    PinName stepPin;
+    uint32_t timerChan;
+    const uint32_t maxFreq = 100000;
+    volatile uint32_t prevFreq1 = 0;
+    volatile uint32_t prevFreq2 = 0;
+
+public:
+    static uint32_t sync0CycleTime;
+    volatile uint32_t pwmCycleTime;
+
+    StepGen(TIM_TypeDef *Timer, uint32_t timerChannel, PinName stepPin, uint8_t dirPin, void irq(void));
+    void reqPos(double_t pos);
+    double reqPos();
+    void actPos(double_t pos);
+    double actPos();
+    void handleStepper(void);
+    void timerCB();
+    void setScale(int16_t spm);
+    void enable(uint8_t yes);
+};
+
+#endif

Cards/EaserCAT-2000/Firmware/include/StepGen2.h

@@ -0,0 +1,53 @@
+
+#ifndef STEPGEN
+#define STEPGEN
+#include <HardwareTimer.h>
+
+class StepGen2
+{
+public:
+    volatile double_t actualPosition;
+    volatile int32_t nSteps;
+    volatile uint32_t timerFrequency;
+    volatile int32_t timerPosition = 0;
+    volatile int32_t timerEndPosition = 0;
+
+public:
+    volatile float Tstartf;    // Starting delay in secs
+    volatile uint32_t Tstartu; // Starting delay in usecs
+    volatile float Tpulses;    // Time it takes to do pulses. Debug
+
+    HardwareTimer *pulseTimer;
+    uint32_t pulseTimerChan;
+    HardwareTimer *startTimer; // Use timers 10,11,13,14
+    uint8_t dirPin;
+    PinName dirPinName;
+    PinName stepPin;
+    uint32_t Tjitter = 400; // Longest time from IRQ to handling in handleStepper, unit is microseconds
+    uint64_t dbg;
+    const uint16_t t2 = 5;                                            // DIR is ahead of PUL with at least 5 usecs
+    const uint16_t t3 = 5;                                            // Pulse width at least 2.5 usecs
+    const uint16_t t4 = 5;                                            // Low level width not less than 2.5 usecs
+    const float maxAllowedFrequency = 1000000 / float(t3 + t4) * 0.9; // 150 kHz for now
+
+public:
+    volatile double_t commandedPosition;    // End position when this cycle is completed
+    volatile int32_t commandedStepPosition; // End step position when this cycle is completed
+    volatile double_t initialPosition;      // From previous cycle
+    volatile int32_t initialStepPosition;   // From previous cycle
+    int16_t stepsPerMM;                     // This many steps per mm
+    volatile uint8_t enabled;               // Enabled step generator
+    volatile float frequency;
+
+    static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds
+    volatile float lcncCycleTime;   // Linuxcnc nominal cycle time in sec (1 ms often)
+
+    StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
+
+    uint32_t handleStepper(uint64_t irqTime /* time when irq happened nanosecs */, uint16_t nLoops);
+    void startTimerCB();
+    void pulseTimerCB();
+    uint32_t updatePos(uint32_t i);
+};
+
+#endif

Cards/EaserCAT-2000/Firmware/include/StepGen3.h

@@ -0,0 +1,156 @@
+
+#ifndef STEPGEN3
+#define STEPGEN3
+#include <HardwareTimer.h>
+
+#define MAX_CHAN 16
+#define MAX_CYCLE 18
+#define USER_STEP_TYPE 13
+
+typedef struct
+{
+    /* stuff that is both read and written by makepulses */
+    volatile unsigned int timer1; /* times out when step pulse should end */
+    volatile unsigned int timer2; /* times out when safe to change dir */
+    volatile unsigned int timer3; /* times out when safe to step in new dir */
+    volatile int hold_dds;        /* prevents accumulator from updating */
+    volatile long addval;         /* actual frequency generator add value */
+    volatile long long accum;     /* frequency generator accumulator */
+    volatile int rawcount;        /* param: position feedback in counts */
+    volatile int curr_dir;        /* current direction */
+    volatile int state;           /* current position in state table */
+    /* stuff that is read but not written by makepulses */
+    volatile int enable;                /* pin for enable stepgen */
+    volatile int target_addval;         /* desired freq generator add value */
+    volatile long deltalim;             /* max allowed change per period */
+    volatile int step_len;              /* parameter: step pulse length */
+    volatile unsigned int dir_hold_dly; /* param: direction hold time or delay */
+    volatile unsigned int dir_setup;    /* param: direction setup time */
+    volatile int step_type;             /* stepping type - see list above */
+    volatile int cycle_max;             /* cycle length for step types 2 and up */
+    volatile int num_phases;            /* number of phases for types 2 and up */
+    volatile int phase[5];              /* pins for output signals */
+    volatile const unsigned char *lut;  /* pointer to state lookup table */
+                                        /* stuff that is not accessed by makepulses */
+    int pos_mode;                       /* 1 = position mode, 0 = velocity mode */
+    unsigned int step_space;            /* parameter: min step pulse spacing */
+    double old_pos_cmd;                 /* previous position command (counts) */
+    int count;                          /* pin: captured feedback in counts */
+#define double float
+    double pos_scale;          /* param: steps per position unit */
+    double old_scale;          /* stored scale value */
+    double scale_recip;        /* reciprocal value used for scaling */
+    double vel_cmd;            /* pin: velocity command (pos units/sec) */
+    double pos_cmd;            /* pin: position command (position units) */
+    double pos_fb;             /* pin: position feedback (position units) */
+    double freq;               /* param: frequency command */
+    double maxvel;             /* param: max velocity, (pos units/sec) */
+    double maxaccel;           /* param: max accel (pos units/sec^2) */
+    unsigned int old_step_len; /* used to detect parameter changes */
+    unsigned int old_step_space;
+    unsigned int old_dir_hold_dly;
+    unsigned int old_dir_setup;
+    int printed_error; /* flag to avoid repeated printing */
+} stepgen_t;
+
+#define MAX_STEP_TYPE 15
+
+#define STEP_PIN 0 /* output phase used for STEP signal */
+#define DIR_PIN 1  /* output phase used for DIR signal */
+#define UP_PIN 0   /* output phase used for UP signal */
+#define DOWN_PIN 1 /* output phase used for DOWN signal */
+
+#define PICKOFF 28 /* bit location in DDS accum */
+
+typedef enum CONTROL
+{
+    POSITION,
+    VELOCITY,
+    INVALID
+} CONTROL;
+
+class StepGen3
+{
+public:
+    int step_type[MAX_CHAN] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1};               // stepping types for up to 16 channels
+    char *ctrl_type[MAX_CHAN] = {0};                                                                          // control type ("p"pos or "v"vel) for up to 16 channels
+    int user_step_type[MAX_CYCLE] = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // lookup table for user-defined step type
+    uint32_t stepPin[MAX_CHAN] = {0};
+    uint32_t dirPin[MAX_CHAN] = {0};
+
+    stepgen_t *stepgen_array = 0;
+
+    int num_chan = 0;          // number of step generators configured */
+    long periodns;             // makepulses function period in nanosec */
+    long old_periodns;         // used to detect changes in periodns */
+    double periodfp;           // makepulses function period in seconds */
+    double freqscale;          // conv. factor from Hz to addval counts */
+    double accelscale;         // conv. Hz/sec to addval cnts/period */
+    long old_dtns;             // update_freq funct period in nsec */
+    double dt;                 // update_freq period in seconds */
+    double recip_dt;           // recprocal of period, avoids divides */
+    volatile uint64_t cnt = 0; // Debug counter
+#undef double
+    StepGen3(void);
+    void updateStepGen(double pos_cmd1, double pos_cmd2, uint32_t servoPeriod);
+    void makeAllPulses(void);
+    int rtapi_app_main();
+    int export_stepgen(int num, stepgen_t *addr, int step_type, int pos_mode);
+    void make_pulses(void *arg, long period);
+    void update_freq(void *arg, long period);
+    void update_pos(void *arg, long period);
+    int setup_user_step_type(void);
+    CONTROL parse_ctrl_type(const char *ctrl);
+    unsigned long ulceil(unsigned long value, unsigned long increment);
+
+private:
+    /* lookup tables for stepping types 2 and higher - phase A is the LSB */
+
+    unsigned char master_lut[MAX_STEP_TYPE][MAX_CYCLE] = {
+        {1, 3, 2, 0, 0, 0, 0, 0, 0, 0},        /* type 2: Quadrature */
+        {1, 2, 4, 0, 0, 0, 0, 0, 0, 0},        /* type 3: Three Wire */
+        {1, 3, 2, 6, 4, 5, 0, 0, 0, 0},        /* type 4: Three Wire Half Step */
+        {1, 2, 4, 8, 0, 0, 0, 0, 0, 0},        /* 5: Unipolar Full Step 1 */
+        {3, 6, 12, 9, 0, 0, 0, 0, 0, 0},       /* 6: Unipoler Full Step 2 */
+        {1, 7, 14, 8, 0, 0, 0, 0, 0, 0},       /* 7: Bipolar Full Step 1 */
+        {5, 6, 10, 9, 0, 0, 0, 0, 0, 0},       /* 8: Bipoler Full Step 2 */
+        {1, 3, 2, 6, 4, 12, 8, 9, 0, 0},       /* 9: Unipolar Half Step */
+        {1, 5, 7, 6, 14, 10, 8, 9, 0, 0},      /* 10: Bipolar Half Step */
+        {1, 2, 4, 8, 16, 0, 0, 0, 0, 0},       /* 11: Five Wire Unipolar */
+        {3, 6, 12, 24, 17, 0, 0, 0, 0, 0},     /* 12: Five Wire Wave */
+        {1, 3, 2, 6, 4, 12, 8, 24, 16, 17},    /* 13: Five Wire Uni Half */
+        {3, 7, 6, 14, 12, 28, 24, 25, 17, 19}, /* 14: Five Wire Wave Half */
+        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}         /* 15: User-defined */
+    };
+
+    unsigned char cycle_len_lut[14] =
+        {4, 3, 6, 4, 4, 4, 4, 8, 8, 5, 5, 10, 10, 0};
+
+    unsigned char num_phases_lut[14] =
+        {
+            2,
+            3,
+            3,
+            4,
+            4,
+            4,
+            4,
+            4,
+            4,
+            5,
+            5,
+            5,
+            5,
+            0,
+    };
+};
+
+// For the example
+#define BASE_PERIOD 20000 
+//#define SERVO_PERIOD 2000000
+#define JOINT_X_STEPGEN_MAXACCEL 0.0
+#define JOINT_Z_STEPGEN_MAXACCEL 0.0
+#define JOINT_X_SCALE -200
+#define JOINT_Z_SCALE -80
+
+#endif

Cards/EaserCAT-2000/Firmware/include/Stm32F4_Encoder.h

@@ -0,0 +1,652 @@
+
+#ifndef __Stm32F4_Encoder_H__
+#define __Stm32F4_Encoder_H__
+#include <Arduino.h>
+
+#define GPIO_Speed_50MHz 0x02 /*!< Fast speed */
+
+// #define GPIO_MODER_MODER0                    ((uint32_t)0x00000003)
+#define GPIO_Mode_OUT 0x01
+// #define GPIO_OSPEEDER_OSPEEDR0               ((uint32_t)0x00000003)
+// #define GPIO_OTYPER_OT_0                     ((uint32_t)0x00000001)
+// #define GPIO_PUPDR_PUPDR0                    ((uint32_t)0x00000003)
+
+#define GPIO_PuPd_NOPULL 0x00
+#define GPIO_Mode_AF 0x02
+#define GPIO_OType_PP 0x00
+#define GPIO_PuPd_NOPULL 0x00
+#define GPIO_Pin_0 ((uint16_t)0x0001)   /* Pin 0 selected */
+#define GPIO_Pin_1 ((uint16_t)0x0002)   /* Pin 1 selected */
+#define GPIO_Pin_2 ((uint16_t)0x0004)   /* Pin 2 selected */
+#define GPIO_Pin_3 ((uint16_t)0x0008)   /* Pin 3 selected */
+#define GPIO_Pin_4 ((uint16_t)0x0010)   /* Pin 4 selected */
+#define GPIO_Pin_5 ((uint16_t)0x0020)   /* Pin 5 selected */
+#define GPIO_Pin_6 ((uint16_t)0x0040)   /* Pin 6 selected */
+#define GPIO_Pin_7 ((uint16_t)0x0080)   /* Pin 7 selected */
+#define GPIO_Pin_8 ((uint16_t)0x0100)   /* Pin 8 selected */
+#define GPIO_Pin_9 ((uint16_t)0x0200)   /* Pin 9 selected */
+#define GPIO_Pin_10 ((uint16_t)0x0400)  /* Pin 10 selected */
+#define GPIO_Pin_11 ((uint16_t)0x0800)  /* Pin 11 selected */
+#define GPIO_Pin_12 ((uint16_t)0x1000)  /* Pin 12 selected */
+#define GPIO_Pin_13 ((uint16_t)0x2000)  /* Pin 13 selected */
+#define GPIO_Pin_14 ((uint16_t)0x4000)  /* Pin 14 selected */
+#define GPIO_Pin_15 ((uint16_t)0x8000)  /* Pin 15 selected */
+#define GPIO_Pin_All ((uint16_t)0xFFFF) /* All pins selected */
+
+typedef struct TIM_TimeBaseInitTypeDef
+{
+  uint16_t TIM_Prescaler; /*!< Specifies the prescaler value used to divide the TIM clock.
+                               This parameter can be a number between 0x0000 and 0xFFFF */
+
+  uint16_t TIM_CounterMode; /*!< Specifies the counter mode.
+                                 This parameter can be a value of @ref TIM_Counter_Mode */
+
+  uint32_t TIM_Period; /*!< Specifies the period value to be loaded into the active
+                            Auto-Reload Register at the next update event.
+                            This parameter must be a number between 0x0000 and 0xFFFF.  */
+
+  uint16_t TIM_ClockDivision; /*!< Specifies the clock division.
+                                  This parameter can be a value of @ref TIM_Clock_Division_CKD */
+
+  uint8_t TIM_RepetitionCounter; /*!< Specifies the repetition counter value. Each time the RCR downcounter
+                                      reaches zero, an update event is generated and counting restarts
+                                      from the RCR value (N).
+                                      This means in PWM mode that (N+1) corresponds to:
+                                         - the number of PWM periods in edge-aligned mode
+                                         - the number of half PWM period in center-aligned mode
+                                      This parameter must be a number between 0x00 and 0xFF.
+                                      @note This parameter is valid only for TIM1 and TIM8. */
+} TIM_TimeBaseInitTypeDef;
+
+typedef struct
+{
+  uint16_t setcount;
+
+} encoder;
+
+typedef struct
+{
+  uint16_t TIM_OCMode; /*!< Specifies the TIM mode.
+                            This parameter can be a value of @ref TIM_Output_Compare_and_PWM_modes */
+
+  uint16_t TIM_OutputState; /*!< Specifies the TIM Output Compare state.
+                                 This parameter can be a value of @ref TIM_Output_Compare_State */
+
+  uint16_t TIM_OutputNState; /*!< Specifies the TIM complementary Output Compare state.
+                                  This parameter can be a value of @ref TIM_Output_Compare_N_State
+                                  @note This parameter is valid only for TIM1 and TIM8. */
+
+  uint32_t TIM_Pulse; /*!< Specifies the pulse value to be loaded into the Capture Compare Register.
+                           This parameter can be a number between 0x0000 and 0xFFFF */
+
+  uint16_t TIM_OCPolarity; /*!< Specifies the output polarity.
+                                This parameter can be a value of @ref TIM_Output_Compare_Polarity */
+
+  uint16_t TIM_OCNPolarity; /*!< Specifies the complementary output polarity.
+                                 This parameter can be a value of @ref TIM_Output_Compare_N_Polarity
+                                 @note This parameter is valid only for TIM1 and TIM8. */
+
+  uint16_t TIM_OCIdleState; /*!< Specifies the TIM Output Compare pin state during Idle state.
+                                 This parameter can be a value of @ref TIM_Output_Compare_Idle_State
+                                 @note This parameter is valid only for TIM1 and TIM8. */
+
+  uint16_t TIM_OCNIdleState; /*!< Specifies the TIM Output Compare pin state during Idle state.
+                                  This parameter can be a value of @ref TIM_Output_Compare_N_Idle_State
+                                  @note This parameter is valid only for TIM1 and TIM8. */
+} TIM_OCInitTypeDef;
+
+typedef struct
+{
+
+  uint16_t TIM_Channel; /*!< Specifies the TIM channel.
+                             This parameter can be a value of @ref TIM_Channel */
+
+  uint16_t TIM_ICPolarity; /*!< Specifies the active edge of the input signal.
+                                This parameter can be a value of @ref TIM_Input_Capture_Polarity */
+
+  uint16_t TIM_ICSelection; /*!< Specifies the input.
+                                 This parameter can be a value of @ref TIM_Input_Capture_Selection */
+
+  uint16_t TIM_ICPrescaler; /*!< Specifies the Input Capture Prescaler.
+                                 This parameter can be a value of @ref TIM_Input_Capture_Prescaler */
+
+  uint16_t TIM_ICFilter; /*!< Specifies the input capture filter.
+                              This parameter can be a number between 0x0 and 0xF */
+} TIM_ICInitTypeDef;
+
+typedef struct
+{
+
+  uint16_t TIM_OSSRState; /*!< Specifies the Off-State selection used in Run mode.
+                               This parameter can be a value of @ref TIM_OSSR_Off_State_Selection_for_Run_mode_state */
+
+  uint16_t TIM_OSSIState; /*!< Specifies the Off-State used in Idle state.
+                               This parameter can be a value of @ref TIM_OSSI_Off_State_Selection_for_Idle_mode_state */
+
+  uint16_t TIM_LOCKLevel; /*!< Specifies the LOCK level parameters.
+                               This parameter can be a value of @ref TIM_Lock_level */
+
+  uint16_t TIM_DeadTime; /*!< Specifies the delay time between the switching-off and the
+                              switching-on of the outputs.
+                              This parameter can be a number between 0x00 and 0xFF  */
+
+  uint16_t TIM_Break; /*!< Specifies whether the TIM Break input is enabled or not.
+                           This parameter can be a value of @ref TIM_Break_Input_enable_disable */
+
+  uint16_t TIM_BreakPolarity; /*!< Specifies the TIM Break Input pin polarity.
+                                   This parameter can be a value of @ref TIM_Break_Polarity */
+
+  uint16_t TIM_AutomaticOutput; /*!< Specifies whether the TIM Automatic Output feature is enabled or not.
+                                     This parameter can be a value of @ref TIM_AOE_Bit_Set_Reset */
+} TIM_BDTRInitTypeDef;
+
+#define GPIO_PinSource0 ((uint8_t)0x00)
+#define GPIO_PinSource1 ((uint8_t)0x01)
+#define GPIO_PinSource2 ((uint8_t)0x02)
+#define GPIO_PinSource3 ((uint8_t)0x03)
+#define GPIO_PinSource4 ((uint8_t)0x04)
+#define GPIO_PinSource5 ((uint8_t)0x05)
+#define GPIO_PinSource6 ((uint8_t)0x06)
+#define GPIO_PinSource7 ((uint8_t)0x07)
+#define GPIO_PinSource8 ((uint8_t)0x08)
+#define GPIO_PinSource9 ((uint8_t)0x09)
+#define GPIO_PinSource10 ((uint8_t)0x0A)
+#define GPIO_PinSource11 ((uint8_t)0x0B)
+#define GPIO_PinSource12 ((uint8_t)0x0C)
+#define GPIO_PinSource13 ((uint8_t)0x0D)
+#define GPIO_PinSource14 ((uint8_t)0x0E)
+#define GPIO_PinSource15 ((uint8_t)0x0F)
+#define GPIO_AF_TIM8 ((uint8_t)0x03) /* TIM8 Alternate Function mapping */
+#define GPIO_AF_TIM3 ((uint8_t)0x02) /* TIM3 Alternate Function mapping */
+#define GPIO_AF_TIM4 ((uint8_t)0x02) /* TIM4 Alternate Function mapping */
+#define GPIO_AF_TIM5 ((uint8_t)0x02) /* TIM5 Alternate Function mapping */
+#define GPIO_AF_TIM1 ((uint8_t)0x01) /* TIM1 Alternate Function mapping */
+#define GPIO_AF_TIM2 ((uint8_t)0x01) /* TIM2 Alternate Function mapping */
+
+// #define TIM4 ((TIM_TypeDef *)TIM4_BASE)
+// #define TIM8 ((TIM_TypeDef *)TIM8_BASE)
+#define TIM_EncoderMode_TI12 ((uint16_t)0x0003)
+#define TIM_ICPolarity_Rising ((uint16_t)0x0000)
+#define TIM_ICPolarity_Falling ((uint16_t)0x0002)
+// #define  TIM_CR1_CEN                         ((uint16_t)0x0001)            /*!<Counter enable */
+// #define  TIM_CR1_CEN                         ((uint16_t)0x0001)            /*!<Counter enable */
+
+#define IS_TIM_ALL_PERIPH(PERIPH) (((PERIPH) == TIM1) ||   \
+                                   ((PERIPH) == TIM2) ||   \
+                                   ((PERIPH) == TIM3) ||   \
+                                   ((PERIPH) == TIM4) ||   \
+                                   ((PERIPH) == TIM5) ||   \
+                                   ((PERIPH) == TIM6) ||   \
+                                   ((PERIPH) == TIM7) ||   \
+                                   ((PERIPH) == TIM8) ||   \
+                                   ((PERIPH) == TIM9) ||   \
+                                   ((PERIPH) == TIM10) ||  \
+                                   ((PERIPH) == TIM11) ||  \
+                                   ((PERIPH) == TIM12) ||  \
+                                   (((PERIPH) == TIM13) || \
+                                    ((PERIPH) == TIM14)))
+
+#define IS_TIM_LIST1_PERIPH(PERIPH) (((PERIPH) == TIM1) ||  \
+                                     ((PERIPH) == TIM2) ||  \
+                                     ((PERIPH) == TIM3) ||  \
+                                     ((PERIPH) == TIM4) ||  \
+                                     ((PERIPH) == TIM5) ||  \
+                                     ((PERIPH) == TIM8) ||  \
+                                     ((PERIPH) == TIM9) ||  \
+                                     ((PERIPH) == TIM10) || \
+                                     ((PERIPH) == TIM11) || \
+                                     ((PERIPH) == TIM12) || \
+                                     ((PERIPH) == TIM13) || \
+                                     ((PERIPH) == TIM14))
+
+#define IS_TIM_LIST2_PERIPH(PERIPH) (((PERIPH) == TIM1) || \
+                                     ((PERIPH) == TIM2) || \
+                                     ((PERIPH) == TIM3) || \
+                                     ((PERIPH) == TIM4) || \
+                                     ((PERIPH) == TIM5) || \
+                                     ((PERIPH) == TIM8) || \
+                                     ((PERIPH) == TIM9) || \
+                                     ((PERIPH) == TIM12))
+
+#define IS_TIM_LIST3_PERIPH(PERIPH) (((PERIPH) == TIM1) || \
+                                     ((PERIPH) == TIM2) || \
+                                     ((PERIPH) == TIM3) || \
+                                     ((PERIPH) == TIM4) || \
+                                     ((PERIPH) == TIM5) || \
+                                     ((PERIPH) == TIM8))
+
+#define IS_TIM_LIST4_PERIPH(PERIPH) (((PERIPH) == TIM1) || \
+                                     ((PERIPH) == TIM8))
+
+#define IS_TIM_LIST5_PERIPH(PERIPH) (((PERIPH) == TIM1) || \
+                                     ((PERIPH) == TIM2) || \
+                                     ((PERIPH) == TIM3) || \
+                                     ((PERIPH) == TIM4) || \
+                                     ((PERIPH) == TIM5) || \
+                                     ((PERIPH) == TIM6) || \
+                                     ((PERIPH) == TIM7) || \
+                                     ((PERIPH) == TIM8))
+
+#define IS_TIM_LIST6_PERIPH(TIMx) (((TIMx) == TIM2) || \
+                                   ((TIMx) == TIM5) || \
+                                   ((TIMx) == TIM11))
+
+#define TIM_OCMode_Timing ((uint16_t)0x0000)
+#define TIM_OCMode_Active ((uint16_t)0x0010)
+#define TIM_OCMode_Inactive ((uint16_t)0x0020)
+#define TIM_OCMode_Toggle ((uint16_t)0x0030)
+#define TIM_OCMode_PWM1 ((uint16_t)0x0060)
+#define TIM_OCMode_PWM2 ((uint16_t)0x0070)
+//#define IS_TIM_OC_MODE(MODE) (((MODE) == TIM_OCMode_Timing) || \
+//                              ((MODE) == TIM_OCMode_Active) || \
+//                              ((MODE) == TIM_OCMode_Inactive) || \
+//                              ((MODE) == TIM_OCMode_Toggle)|| \
+//                              ((MODE) == TIM_OCMode_PWM1) || \
+//                              ((MODE) == TIM_OCMode_PWM2))
+#define IS_TIM_OCM(MODE) (((MODE) == TIM_OCMode_Timing) ||       \
+                          ((MODE) == TIM_OCMode_Active) ||       \
+                          ((MODE) == TIM_OCMode_Inactive) ||     \
+                          ((MODE) == TIM_OCMode_Toggle) ||       \
+                          ((MODE) == TIM_OCMode_PWM1) ||         \
+                          ((MODE) == TIM_OCMode_PWM2) ||         \
+                          ((MODE) == TIM_ForcedAction_Active) || \
+                          ((MODE) == TIM_ForcedAction_InActive))
+
+#define TIM_OPMode_Single ((uint16_t)0x0008)
+#define TIM_OPMode_Repetitive ((uint16_t)0x0000)
+//#define IS_TIM_OPM_MODE(MODE) (((MODE) == TIM_OPMode_Single) || \
+//                               ((MODE) == TIM_OPMode_Repetitive))
+
+#define TIM_Channel_1 ((uint16_t)0x0000)
+#define TIM_Channel_2 ((uint16_t)0x0004)
+#define TIM_Channel_3 ((uint16_t)0x0008)
+#define TIM_Channel_4 ((uint16_t)0x000C)
+
+#define IS_TIM_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \
+                                 ((CHANNEL) == TIM_Channel_2) || \
+                                 ((CHANNEL) == TIM_Channel_3) || \
+                                 ((CHANNEL) == TIM_Channel_4))
+
+#define IS_TIM_PWMI_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \
+                                      ((CHANNEL) == TIM_Channel_2))
+#define IS_TIM_COMPLEMENTARY_CHANNEL(CHANNEL) (((CHANNEL) == TIM_Channel_1) || \
+                                               ((CHANNEL) == TIM_Channel_2) || \
+                                               ((CHANNEL) == TIM_Channel_3))
+
+#define TIM_CKD_DIV1 ((uint16_t)0x0000)
+#define TIM_CKD_DIV2 ((uint16_t)0x0100)
+#define TIM_CKD_DIV4 ((uint16_t)0x0200)
+#define IS_TIM_CKD_DIV(DIV) (((DIV) == TIM_CKD_DIV1) || \
+                             ((DIV) == TIM_CKD_DIV2) || \
+                             ((DIV) == TIM_CKD_DIV4))
+
+#define TIM_CounterMode_Up ((uint16_t)0x0000)
+#define TIM_CounterMode_Down ((uint16_t)0x0010)
+#define TIM_CounterMode_CenterAligned1 ((uint16_t)0x0020)
+#define TIM_CounterMode_CenterAligned2 ((uint16_t)0x0040)
+#define TIM_CounterMode_CenterAligned3 ((uint16_t)0x0060)
+//#define IS_TIM_COUNTER_MODE(MODE) (((MODE) == TIM_CounterMode_Up) ||  \
+//                                   ((MODE) == TIM_CounterMode_Down) || \
+//                                   ((MODE) == TIM_CounterMode_CenterAligned1) || \
+//                                   ((MODE) == TIM_CounterMode_CenterAligned2) || \
+//                                   ((MODE) == TIM_CounterMode_CenterAligned3))
+
+#define TIM_OCPolarity_High ((uint16_t)0x0000)
+#define TIM_OCPolarity_Low ((uint16_t)0x0002)
+//#define IS_TIM_OC_POLARITY(POLARITY) (((POLARITY) == TIM_OCPolarity_High) || \
+//                                      ((POLARITY) == TIM_OCPolarity_Low))
+
+#define TIM_OCNPolarity_High ((uint16_t)0x0000)
+#define TIM_OCNPolarity_Low ((uint16_t)0x0008)
+//#define IS_TIM_OCN_POLARITY(POLARITY) (((POLARITY) == TIM_OCNPolarity_High) || \
+//                                       ((POLARITY) == TIM_OCNPolarity_Low))
+
+#define TIM_OutputState_Disable ((uint16_t)0x0000)
+#define TIM_OutputState_Enable ((uint16_t)0x0001)
+#define IS_TIM_OUTPUT_STATE(STATE) (((STATE) == TIM_OutputState_Disable) || \
+                                    ((STATE) == TIM_OutputState_Enable))
+
+#define TIM_OutputNState_Disable ((uint16_t)0x0000)
+#define TIM_OutputNState_Enable ((uint16_t)0x0004)
+#define IS_TIM_OUTPUTN_STATE(STATE) (((STATE) == TIM_OutputNState_Disable) || \
+                                     ((STATE) == TIM_OutputNState_Enable))
+
+#define TIM_CCx_Enable ((uint16_t)0x0001)
+#define TIM_CCx_Disable ((uint16_t)0x0000)
+#define IS_TIM_CCX(CCX) (((CCX) == TIM_CCx_Enable) || \
+                         ((CCX) == TIM_CCx_Disable))
+
+#define TIM_CCxN_Enable ((uint16_t)0x0004)
+#define TIM_CCxN_Disable ((uint16_t)0x0000)
+#define IS_TIM_CCXN(CCXN) (((CCXN) == TIM_CCxN_Enable) || \
+                           ((CCXN) == TIM_CCxN_Disable))
+
+#define TIM_Break_Enable ((uint16_t)0x1000)
+#define TIM_Break_Disable ((uint16_t)0x0000)
+//#define IS_TIM_BREAK_STATE(STATE) (((STATE) == TIM_Break_Enable) || \
+//                                   ((STATE) == TIM_Break_Disable))
+
+#define TIM_BreakPolarity_Low ((uint16_t)0x0000)
+#define TIM_BreakPolarity_High ((uint16_t)0x2000)
+//#define IS_TIM_BREAK_POLARITY(POLARITY) (((POLARITY) == TIM_BreakPolarity_Low) || \
+//    ((POLARITY) == TIM_BreakPolarity_High))
+
+#define TIM_AutomaticOutput_Enable ((uint16_t)0x4000)
+#define TIM_AutomaticOutput_Disable ((uint16_t)0x0000)
+//#define IS_TIM_AUTOMATIC_OUTPUT_STATE(STATE) (((STATE) == TIM_AutomaticOutput_Enable) || \
+//    ((STATE) == TIM_AutomaticOutput_Disable))
+
+#define TIM_LOCKLevel_OFF ((uint16_t)0x0000)
+#define TIM_LOCKLevel_1 ((uint16_t)0x0100)
+#define TIM_LOCKLevel_2 ((uint16_t)0x0200)
+#define TIM_LOCKLevel_3 ((uint16_t)0x0300)
+//#define IS_TIM_LOCK_LEVEL(LEVEL) (((LEVEL) == TIM_LOCKLevel_OFF) || \
+//                                  ((LEVEL) == TIM_LOCKLevel_1) || \
+//                                  ((LEVEL) == TIM_LOCKLevel_2) || \
+//                                  ((LEVEL) == TIM_LOCKLevel_3))
+
+#define TIM_OSSIState_Enable ((uint16_t)0x0400)
+#define TIM_OSSIState_Disable ((uint16_t)0x0000)
+//#define IS_TIM_OSSI_STATE(STATE) (((STATE) == TIM_OSSIState_Enable) || \
+//                                  ((STATE) == TIM_OSSIState_Disable))
+
+#define TIM_OSSRState_Enable ((uint16_t)0x0800)
+#define TIM_OSSRState_Disable ((uint16_t)0x0000)
+//#define IS_TIM_OSSR_STATE(STATE) (((STATE) == TIM_OSSRState_Enable) || \
+//                                  ((STATE) == TIM_OSSRState_Disable))
+
+#define TIM_OCIdleState_Set ((uint16_t)0x0100)
+#define TIM_OCIdleState_Reset ((uint16_t)0x0000)
+//#define IS_TIM_OCIDLE_STATE(STATE) (((STATE) == TIM_OCIdleState_Set) || \
+//                                    ((STATE) == TIM_OCIdleState_Reset))
+//
+
+#define TIM_OCNIdleState_Set ((uint16_t)0x0200)
+#define TIM_OCNIdleState_Reset ((uint16_t)0x0000)
+//#define IS_TIM_OCNIDLE_STATE(STATE) (((STATE) == TIM_OCNIdleState_Set) || \
+//                                     ((STATE) == TIM_OCNIdleState_Reset))
+
+#define TIM_ICPolarity_Rising ((uint16_t)0x0000)
+#define TIM_ICPolarity_Falling ((uint16_t)0x0002)
+#define TIM_ICPolarity_BothEdge ((uint16_t)0x000A)
+//#define IS_TIM_IC_POLARITY(POLARITY) (((POLARITY) == TIM_ICPolarity_Rising) || \
+//                                      ((POLARITY) == TIM_ICPolarity_Falling)|| \
+//                                      ((POLARITY) == TIM_ICPolarity_BothEdge))
+
+#define TIM_ICSelection_DirectTI ((uint16_t)0x0001)   /*!< TIM Input 1, 2, 3 or 4 is selected to be \
+                                                           connected to IC1, IC2, IC3 or IC4, respectively */
+#define TIM_ICSelection_IndirectTI ((uint16_t)0x0002) /*!< TIM Input 1, 2, 3 or 4 is selected to be \
+                                                           connected to IC2, IC1, IC4 or IC3, respectively. */
+#define TIM_ICSelection_TRC ((uint16_t)0x0003)        /*!< TIM Input 1, 2, 3 or 4 is selected to be connected to TRC. */
+//#define IS_TIM_IC_SELECTION(SELECTION) (((SELECTION) == TIM_ICSelection_DirectTI) || \
+//                                        ((SELECTION) == TIM_ICSelection_IndirectTI) || \
+//                                        ((SELECTION) == TIM_ICSelection_TRC))
+
+// #define TIM_ICPSC_DIV1                     ((uint16_t)0x0000) /*!< Capture performed each time an edge is detected on the capture input. */
+// #define TIM_ICPSC_DIV2                     ((uint16_t)0x0004) /*!< Capture performed once every 2 events. */
+// #define TIM_ICPSC_DIV4                     ((uint16_t)0x0008) /*!< Capture performed once every 4 events. */
+// #define TIM_ICPSC_DIV8                     ((uint16_t)0x000C) /*!< Capture performed once every 8 events. */
+// #define IS_TIM_IC_PRESCALER(PRESCALER) ( ((PRESCALER) == TIM_ICPSC_DIV1) || \
+//                                        ((PRESCALER) == TIM_ICPSC_DIV2) || \
+//                                        ((PRESCALER) == TIM_ICPSC_DIV4) || \
+//                                        ((PRESCALER) == TIM_ICPSC_DIV8))
+
+#define TIM_IT_Update ((uint16_t)0x0001)
+// #define TIM_IT_CC1                         ((uint16_t)0x0002)
+// #define TIM_IT_CC2                         ((uint16_t)0x0004)
+// #define TIM_IT_CC3                         ((uint16_t)0x0008)
+// #define TIM_IT_CC4                         ((uint16_t)0x0010)
+// #define TIM_IT_COM                         ((uint16_t)0x0020)
+#define TIM_IT_Trigger ((uint16_t)0x0040)
+#define TIM_IT_Break ((uint16_t)0x0080)
+#define IS_TIM_IT(IT) ((((IT) & (uint16_t)0xFF00) == 0x0000) && ((IT) != 0x0000))
+
+#define IS_TIM_GET_IT(IT) (((IT) == TIM_IT_Update) ||  \
+                           ((IT) == TIM_IT_CC1) ||     \
+                           ((IT) == TIM_IT_CC2) ||     \
+                           ((IT) == TIM_IT_CC3) ||     \
+                           ((IT) == TIM_IT_CC4) ||     \
+                           ((IT) == TIM_IT_COM) ||     \
+                           ((IT) == TIM_IT_Trigger) || \
+                           ((IT) == TIM_IT_Break))
+
+// #define TIM_DMABase_CR1                    ((uint16_t)0x0000)
+// #define TIM_DMABase_CR2                    ((uint16_t)0x0001)
+// #define TIM_DMABase_SMCR                   ((uint16_t)0x0002)
+// #define TIM_DMABase_DIER                   ((uint16_t)0x0003)
+// #define TIM_DMABase_SR                     ((uint16_t)0x0004)
+// #define TIM_DMABase_EGR                    ((uint16_t)0x0005)
+// #define TIM_DMABase_CCMR1                  ((uint16_t)0x0006)
+// #define TIM_DMABase_CCMR2                  ((uint16_t)0x0007)
+// #define TIM_DMABase_CCER                   ((uint16_t)0x0008)
+// #define TIM_DMABase_CNT                    ((uint16_t)0x0009)
+// #define TIM_DMABase_PSC                    ((uint16_t)0x000A)
+// #define TIM_DMABase_ARR                    ((uint16_t)0x000B)
+// #define TIM_DMABase_RCR                    ((uint16_t)0x000C)
+// #define TIM_DMABase_CCR1                   ((uint16_t)0x000D)
+// #define TIM_DMABase_CCR2                   ((uint16_t)0x000E)
+// #define TIM_DMABase_CCR3                   ((uint16_t)0x000F)
+// #define TIM_DMABase_CCR4                   ((uint16_t)0x0010)
+// #define TIM_DMABase_BDTR                   ((uint16_t)0x0011)
+// #define TIM_DMABase_DCR                    ((uint16_t)0x0012)
+// #define TIM_DMABase_OR                     ((uint16_t)0x0013)
+// #define IS_TIM_DMA_BASE(BASE) ( ((BASE) == TIM_DMABase_CR1) || \
+//                               ((BASE) == TIM_DMABase_CR2) || \
+//                               ((BASE) == TIM_DMABase_SMCR) || \
+//                               ((BASE) == TIM_DMABase_DIER) || \
+//                               ((BASE) == TIM_DMABase_SR) || \
+//                               ((BASE) == TIM_DMABase_EGR) || \
+//                               ((BASE) == TIM_DMABase_CCMR1) || \
+//                               ((BASE) == TIM_DMABase_CCMR2) || \
+//                               ((BASE) == TIM_DMABase_CCER) || \
+//                               ((BASE) == TIM_DMABase_CNT) || \
+//                               ((BASE) == TIM_DMABase_PSC) || \
+//                               ((BASE) == TIM_DMABase_ARR) || \
+//                               ((BASE) == TIM_DMABase_RCR) || \
+//                               ((BASE) == TIM_DMABase_CCR1) || \
+//                               ((BASE) == TIM_DMABase_CCR2) || \
+//                               ((BASE) == TIM_DMABase_CCR3) || \
+//                               ((BASE) == TIM_DMABase_CCR4) || \
+//                               ((BASE) == TIM_DMABase_BDTR) || \
+//                               ((BASE) == TIM_DMABase_DCR) || \
+//                               ((BASE) == TIM_DMABase_OR))
+
+// #define TIM_DMABurstLength_1Transfer           ((uint16_t)0x0000)
+// #define TIM_DMABurstLength_2Transfers          ((uint16_t)0x0100)
+// #define TIM_DMABurstLength_3Transfers          ((uint16_t)0x0200)
+// #define TIM_DMABurstLength_4Transfers          ((uint16_t)0x0300)
+// #define TIM_DMABurstLength_5Transfers          ((uint16_t)0x0400)
+// #define TIM_DMABurstLength_6Transfers          ((uint16_t)0x0500)
+// #define TIM_DMABurstLength_7Transfers          ((uint16_t)0x0600)
+// #define TIM_DMABurstLength_8Transfers          ((uint16_t)0x0700)
+// #define TIM_DMABurstLength_9Transfers          ((uint16_t)0x0800)
+// #define TIM_DMABurstLength_10Transfers         ((uint16_t)0x0900)
+// #define TIM_DMABurstLength_11Transfers         ((uint16_t)0x0A00)
+// #define TIM_DMABurstLength_12Transfers         ((uint16_t)0x0B00)
+// #define TIM_DMABurstLength_13Transfers         ((uint16_t)0x0C00)
+// #define TIM_DMABurstLength_14Transfers         ((uint16_t)0x0D00)
+// #define TIM_DMABurstLength_15Transfers         ((uint16_t)0x0E00)
+// #define TIM_DMABurstLength_16Transfers         ((uint16_t)0x0F00)
+// #define TIM_DMABurstLength_17Transfers         ((uint16_t)0x1000)
+// #define TIM_DMABurstLength_18Transfers         ((uint16_t)0x1100)
+// #define IS_TIM_DMA_LENGTH(LENGTH) ( ((LENGTH) == TIM_DMABurstLength_1Transfer) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_2Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_3Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_4Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_5Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_6Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_7Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_8Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_9Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_10Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_11Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_12Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_13Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_14Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_15Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_16Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_17Transfers) || \
+//                                   ((LENGTH) == TIM_DMABurstLength_18Transfers))
+
+// #define TIM_DMA_Update                     ((uint16_t)0x0100)
+// #define TIM_DMA_CC1                        ((uint16_t)0x0200)
+// #define TIM_DMA_CC2                        ((uint16_t)0x0400)
+// #define TIM_DMA_CC3                        ((uint16_t)0x0800)
+// #define TIM_DMA_CC4                        ((uint16_t)0x1000)
+// #define TIM_DMA_COM                        ((uint16_t)0x2000)
+// #define TIM_DMA_Trigger                    ((uint16_t)0x4000)
+// #define IS_TIM_DMA_SOURCE(SOURCE) ((((SOURCE) & (uint16_t)0x80FF) == 0x0000) && ((SOURCE) != 0x0000))
+
+#define TIM_ExtTRGPSC_OFF ((uint16_t)0x0000)
+#define TIM_ExtTRGPSC_DIV2 ((uint16_t)0x1000)
+#define TIM_ExtTRGPSC_DIV4 ((uint16_t)0x2000)
+#define TIM_ExtTRGPSC_DIV8 ((uint16_t)0x3000)
+#define IS_TIM_EXT_PRESCALER(PRESCALER) (((PRESCALER) == TIM_ExtTRGPSC_OFF) ||  \
+                                         ((PRESCALER) == TIM_ExtTRGPSC_DIV2) || \
+                                         ((PRESCALER) == TIM_ExtTRGPSC_DIV4) || \
+                                         ((PRESCALER) == TIM_ExtTRGPSC_DIV8))
+
+// #define TIM_TS_ITR0                        ((uint16_t)0x0000)
+// #define TIM_TS_ITR1                        ((uint16_t)0x0010)
+// #define TIM_TS_ITR2                        ((uint16_t)0x0020)
+// #define TIM_TS_ITR3                        ((uint16_t)0x0030)
+// #define TIM_TS_TI1F_ED                     ((uint16_t)0x0040)
+// #define TIM_TS_TI1FP1                      ((uint16_t)0x0050)
+// #define TIM_TS_TI2FP2                      ((uint16_t)0x0060)
+// #define TIM_TS_ETRF                        ((uint16_t)0x0070)
+// #def ine IS_TIM_TRIGGER_SELECTION(SELECTION) (((SELECTION) == TIM_TS_ITR0) || \
+//    ((SELECTION) == TIM_TS_ITR1) || \
+//    ((SELECTION) == TIM_TS_ITR2) || \
+//    ((SELECTION) == TIM_TS_ITR3) || \
+//    ((SELECTION) == TIM_TS_TI1F_ED) || \
+//    ((SELECTION) == TIM_TS_TI1FP1) || \
+//    ((SELECTION) == TIM_TS_TI2FP2) || \
+//    ((SELECTION) == TIM_TS_ETRF))
+#define IS_TIM_INTERNAL_TRIGGER_SELECTION(SELECTION) (((SELECTION) == TIM_TS_ITR0) || \
+                                                      ((SELECTION) == TIM_TS_ITR1) || \
+                                                      ((SELECTION) == TIM_TS_ITR2) || \
+                                                      ((SELECTION) == TIM_TS_ITR3))
+
+#define TIM_TIxExternalCLK1Source_TI1 ((uint16_t)0x0050)
+#define TIM_TIxExternalCLK1Source_TI2 ((uint16_t)0x0060)
+#define TIM_TIxExternalCLK1Source_TI1ED ((uint16_t)0x0040)
+
+#define TIM_ExtTRGPolarity_Inverted ((uint16_t)0x8000)
+#define TIM_ExtTRGPolarity_NonInverted ((uint16_t)0x0000)
+#define IS_TIM_EXT_POLARITY(POLARITY) (((POLARITY) == TIM_ExtTRGPolarity_Inverted) || \
+                                       ((POLARITY) == TIM_ExtTRGPolarity_NonInverted))
+
+#define TIM_PSCReloadMode_Update ((uint16_t)0x0000)
+#define TIM_PSCReloadMode_Immediate ((uint16_t)0x0001)
+#define IS_TIM_PRESCALER_RELOAD(RELOAD) (((RELOAD) == TIM_PSCReloadMode_Update) || \
+                                         ((RELOAD) == TIM_PSCReloadMode_Immediate))
+
+#define TIM_ForcedAction_Active ((uint16_t)0x0050)
+#define TIM_ForcedAction_InActive ((uint16_t)0x0040)
+#define IS_TIM_FORCED_ACTION(ACTION) (((ACTION) == TIM_ForcedAction_Active) || \
+                                      ((ACTION) == TIM_ForcedAction_InActive))
+
+#define TIM_EncoderMode_TI1 ((uint16_t)0x0001)
+#define TIM_EncoderMode_TI2 ((uint16_t)0x0002)
+#define TIM_EncoderMode_TI12 ((uint16_t)0x0003)
+//#define IS_TIM_ENCODER_MODE(MODE) (((MODE) == TIM_EncoderMode_TI1) || \
+//                                   ((MODE) == TIM_EncoderMode_TI2) || \
+//                                   ((MODE) == TIM_EncoderMode_TI12))
+
+// #define TIM_EventSource_Update             ((uint16_t)0x0001)
+// #define TIM_EventSource_CC1                ((uint16_t)0x0002)
+// #define TIM_EventSource_CC2                ((uint16_t)0x0004)
+// #define TIM_EventSource_CC3                ((uint16_t)0x0008)
+// #define TIM_EventSource_CC4                ((uint16_t)0x0010)
+// #define TIM_EventSource_COM                ((uint16_t)0x0020)
+// #define TIM_EventSource_Trigger            ((uint16_t)0x0040)
+// #define TIM_EventSource_Break              ((uint16_t)0x0080)
+// #define IS_TIM_EVENT_SOURCE(SOURCE) ((((SOURCE) & (uint16_t)0xFF00) == 0x0000) && ((SOURCE) != 0x0000))
+
+#define TIM_UpdateSource_Global ((uint16_t)0x0000)  /*!< Source of update is the counter overflow/underflow \
+                                                         or the setting of UG bit, or an update generation  \
+                                                         through the slave mode controller. */
+#define TIM_UpdateSource_Regular ((uint16_t)0x0001) /*!< Source of update is counter overflow/underflow. */
+#define IS_TIM_UPDATE_SOURCE(SOURCE) (((SOURCE) == TIM_UpdateSource_Global) || \
+                                      ((SOURCE) == TIM_UpdateSource_Regular))
+
+#define TIM_OCPreload_Enable ((uint16_t)0x0008)
+#define TIM_OCPreload_Disable ((uint16_t)0x0000)
+#define IS_TIM_OCPRELOAD_STATE(STATE) (((STATE) == TIM_OCPreload_Enable) || \
+                                       ((STATE) == TIM_OCPreload_Disable))
+
+#define TIM_OCFast_Enable ((uint16_t)0x0004)
+#define TIM_OCFast_Disable ((uint16_t)0x0000)
+#define IS_TIM_OCFAST_STATE(STATE) (((STATE) == TIM_OCFast_Enable) || \
+                                    ((STATE) == TIM_OCFast_Disable))
+
+#define TIM_OCClear_Enable ((uint16_t)0x0080)
+#define TIM_OCClear_Disable ((uint16_t)0x0000)
+#define IS_TIM_OCCLEAR_STATE(STATE) (((STATE) == TIM_OCClear_Enable) || \
+                                     ((STATE) == TIM_OCClear_Disable))
+
+#define TIM_TRGOSource_Reset ((uint16_t)0x0000)
+#define TIM_TRGOSource_Enable ((uint16_t)0x0010)
+#define TIM_TRGOSource_Update ((uint16_t)0x0020)
+#define TIM_TRGOSource_OC1 ((uint16_t)0x0030)
+#define TIM_TRGOSource_OC1Ref ((uint16_t)0x0040)
+#define TIM_TRGOSource_OC2Ref ((uint16_t)0x0050)
+#define TIM_TRGOSource_OC3Ref ((uint16_t)0x0060)
+#define TIM_TRGOSource_OC4Ref ((uint16_t)0x0070)
+//#define IS_TIM_TRGO_SOURCE(SOURCE) (((SOURCE) == TIM_TRGOSource_Reset) || \
+//                                    ((SOURCE) == TIM_TRGOSource_Enable) || \
+//                                    ((SOURCE) == TIM_TRGOSource_Update) || \
+//                                    ((SOURCE) == TIM_TRGOSource_OC1) || \
+//                                    ((SOURCE) == TIM_TRGOSource_OC1Ref) || \
+//                                    ((SOURCE) == TIM_TRGOSource_OC2Ref) || \
+//                                    ((SOURCE) == TIM_TRGOSource_OC3Ref) || \
+//                                    ((SOURCE) == TIM_TRGOSource_OC4Ref))
+
+#define TIM_SlaveMode_Reset ((uint16_t)0x0004)
+#define TIM_SlaveMode_Gated ((uint16_t)0x0005)
+#define TIM_SlaveMode_Trigger ((uint16_t)0x0006)
+#define TIM_SlaveMode_External1 ((uint16_t)0x0007)
+//#define IS_TIM_SLAVE_MODE(MODE) (((MODE) == TIM_SlaveMode_Reset) || \
+//                                 ((MODE) == TIM_SlaveMode_Gated) || \
+//                                 ((MODE) == TIM_SlaveMode_Trigger) || \
+//                                 ((MODE) == TIM_SlaveMode_External1))
+
+#define TIM_MasterSlaveMode_Enable ((uint16_t)0x0080)
+#define TIM_MasterSlaveMode_Disable ((uint16_t)0x0000)
+//#define IS_TIM_MSM_STATE(STATE) (((STATE) == TIM_MasterSlaveMode_Enable) || \
+//                                 ((STATE) == TIM_MasterSlaveMode_Disable))
+
+#define TIM2_TIM8_TRGO ((uint16_t)0x0000)
+#define TIM2_ETH_PTP ((uint16_t)0x0400)
+#define TIM2_USBFS_SOF ((uint16_t)0x0800)
+#define TIM2_USBHS_SOF ((uint16_t)0x0C00)
+
+#define TIM5_GPIO ((uint16_t)0x0000)
+#define TIM5_LSI ((uint16_t)0x0040)
+#define TIM5_LSE ((uint16_t)0x0080)
+#define TIM5_RTC ((uint16_t)0x00C0)
+
+#define TIM11_GPIO ((uint16_t)0x0000)
+#define TIM11_HSE ((uint16_t)0x0002)
+
+class Encoder
+{
+private:
+  int _pin;
+
+public:
+  TIM_TypeDef *tim_base;
+  Encoder();
+  void SetCount(int64_t Counter);
+  uint16_t GetCount();
+};
+
+void encoder_config();
+void encoder2_config(); // Experimental
+void GpioConfigPortA(GPIO_TypeDef *GPIOx);
+void GpioConfigPortC(GPIO_TypeDef *GPIOx);
+void GpioConfigPortD(GPIO_TypeDef *GPIOx);
+void TIM_EncoderInterConfig(TIM_TypeDef *TIMx, uint16_t TIM_EncoderMode, uint16_t TIM_IC1Polarity, uint16_t TIM_IC2Polarity);
+void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct);
+void TIM_TimeBaseInit(TIM_TypeDef *TIMx, TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct);
+#endif

Cards/EaserCAT-2000/Firmware/include/extend32to64.h

@@ -0,0 +1,14 @@
+#ifndef EXTEND32TO64
+#define EXTEND32TO54
+
+#include <Arduino.h>
+
+class extend32to64
+{
+public:
+    int64_t previousTimeValue = 0;
+    const uint64_t ONE_PERIOD = 4294967296;  // almost UINT32_MAX;
+    const uint64_t HALF_PERIOD = 2147483648; // Half of that
+    int64_t extendTime(uint32_t in);
+};
+#endif

Cards/EaserCAT-2000/Firmware/lib/README

@@ -0,0 +1,46 @@
+
+This directory is intended for project specific (private) libraries.
+PlatformIO will compile them to static libraries and link into executable file.
+
+The source code of each library should be placed in a an own separate directory
+("lib/your_library_name/[here are source files]").
+
+For example, see a structure of the following two libraries `Foo` and `Bar`:
+
+|--lib
+|  |
+|  |--Bar
+|  |  |--docs
+|  |  |--examples
+|  |  |--src
+|  |     |- Bar.c
+|  |     |- Bar.h
+|  |  |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
+|  |
+|  |--Foo
+|  |  |- Foo.c
+|  |  |- Foo.h
+|  |
+|  |- README --> THIS FILE
+|
+|- platformio.ini
+|--src
+   |- main.c
+
+and a contents of `src/main.c`:
+```
+#include <Foo.h>
+#include <Bar.h>
+
+int main (void)
+{
+  ...
+}
+
+```
+
+PlatformIO Library Dependency Finder will find automatically dependent
+libraries scanning project source files.
+
+More information about PlatformIO Library Dependency Finder
+- https://docs.platformio.org/page/librarymanager/ldf.html

Cards/EaserCAT-2000/Firmware/lib/soes/CMakeLists.txt

@@ -0,0 +1,29 @@
+
+# NOTE: add headers to make them show up in an IDE
+add_library (soes
+  esc.c
+  esc.h
+  esc_coe.c
+  esc_coe.h
+  esc_foe.c
+  esc_foe.h
+  esc_eoe.c
+  esc_eoe.h
+  esc_eep.c
+  esc_eep.h
+  ecat_slv.c
+  ecat_slv.h
+  options.h
+  ${HAL_SOURCES}
+  )
+
+include_directories(${HAL_INCLUDES})
+
+install (TARGETS soes DESTINATION bin)
+install (FILES
+  esc.h
+  esc_coe.h
+  esc_foe.h
+  esc_eoe.h
+  esc_eep.h
+  DESTINATION include)

Cards/EaserCAT-2000/Firmware/lib/soes/cc.h

@@ -0,0 +1,88 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+#ifndef CC_H
+#define CC_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <assert.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <sys/param.h>
+#ifdef __linux__
+   #include <endian.h>
+#else
+   #include <machine/endian.h>   
+#endif
+
+#ifndef MIN
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#endif
+
+#ifndef MAX
+#define MAX(a,b) (((a)>(b))?(a):(b))
+#endif
+
+#define CC_PACKED_BEGIN
+#define CC_PACKED_END
+#define CC_PACKED       __attribute__((packed))
+
+#ifdef __rtk__
+#define CC_ASSERT(exp) ASSERT (exp)
+#else
+#define CC_ASSERT(exp) assert (exp)
+#endif
+#define CC_STATIC_ASSERT(exp) _Static_assert (exp, "")
+
+#define CC_DEPRECATED   __attribute__((deprecated))
+
+#define CC_SWAP32(x) __builtin_bswap32 (x)
+#define CC_SWAP16(x) __builtin_bswap16 (x)
+
+#define CC_ATOMIC_SET(var,val)   __atomic_store_n(&var,val,__ATOMIC_SEQ_CST)
+#define CC_ATOMIC_GET(var)       __atomic_load_n(&var,__ATOMIC_SEQ_CST)
+#define CC_ATOMIC_ADD(var,val)   __atomic_add_fetch(&var,val,__ATOMIC_SEQ_CST)
+#define CC_ATOMIC_SUB(var,val)   __atomic_sub_fetch(&var,val,__ATOMIC_SEQ_CST)
+#define CC_ATOMIC_AND(var,val)   __atomic_and_fetch(&var,val,__ATOMIC_SEQ_CST)
+#define CC_ATOMIC_OR(var,val)    __atomic_or_fetch(&var,val,__ATOMIC_SEQ_CST)
+
+#if BYTE_ORDER == BIG_ENDIAN
+#define htoes(x) CC_SWAP16 (x)
+#define htoel(x) CC_SWAP32 (x)
+#else
+#define htoes(x) (x)
+#define htoel(x) (x)
+#endif
+
+#define etohs(x) htoes (x)
+#define etohl(x) htoel (x)
+
+#if BYTE_ORDER == LITTLE_ENDIAN
+#define EC_LITTLE_ENDIAN
+#else
+#define EC_BIG_ENDIAN
+#endif
+
+#ifdef ESC_DEBUG
+#ifdef __rtk__
+#include <rprint.h>
+#define DPRINT(...) rprintp ("soes: "__VA_ARGS__)
+#else
+#include <stdio.h>
+#define DPRINT(...) printf ("soes: "__VA_ARGS__)
+#endif
+#else
+#define DPRINT(...)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* CC_H */

Cards/EaserCAT-2000/Firmware/lib/soes/doc/soes.dox

@@ -0,0 +1,50 @@
+/**
+ * \mainpage Simple Open EtherCAT Slave or SOES
+ *
+ * \section start Tutorial
+ * For a tutorial on SOES  See tutorial.txt
+ * 
+ * \section overview Overview
+ * SOES is an EtherCAT slave stack written in c. Its purpose is to learn and
+ * to use. All users are invited to study the source to get an understanding
+ * how an EtherCAT slave function
+ *
+ * Features as of 1.0.0 :
+ * - Address offset based HAL for easy ESC read/write access via any interface
+ * - Mailbox with data link layer
+ * - CoE
+ * - Object dictionary
+ * - SDO read and write for all sizes including segmented transfers
+ * - Easy portable C-code suited for embedded applications
+ * - Fixed PDO mapping
+ * - FoE with bootstrap template
+ * - Support for Little and Big endian targets.
+ * - Polling for interrupts
+ *
+ * \section legal Legal notice
+ * SOES Simple Open EtherCAT Slave \n
+ * Copyright (C) 2007-2013 Arthur Ketels \n
+ * Copyright (C) 2012-2013 rt-labs \n
+ *
+ * SOES is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License version 2 as published by the Free
+ * Software Foundation.
+ *
+ * SOES is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * As a special exception, if other files instantiate templates or use macros
+ * or inline functions from this file, or you compile this file and link it
+ * with other works to produce a work based on this file, this file does not
+ * by itself cause the resulting work to be covered by the GNU General Public
+ * License. However the source code for this file must still be made available
+ * in accordance with section (3) of the GNU General Public License.
+ *
+ * This exception does not invalidate any other reasons why a work based on
+ * this file might be covered by the GNU General Public License.
+ *
+ * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual
+ * property of, and protected by Beckhoff Automation GmbH.
+ */

Cards/EaserCAT-2000/Firmware/lib/soes/doc/tutorial.txt

@@ -0,0 +1,476 @@
+/** \file
+
+\section general General
+
+The SOES is a library that provides the Micro Controller user application with
+ the means to access the EtherCAT fieldbus communication environment:
+ - EtherCAT State Machine
+ - Mailbox Interfaces
+ - Protocols
+   - CoE
+   - FoE + bootstrap template
+
+Support for mailbox and protocols are typical examples when you need a slave
+stack to control the Application Layer of EtherCAT. The PDI used for such 
+applications is either SPI or some Micro Controller Interface
+   
+The following sections show some basic examples on how to get the SOES up
+and running, as well as a lightweight example on howto design your slave. 
+Since all code is local to the application or global variables, it is possible
+ to tweak and optimize when possible.
+ 
+Our target Application:
+ - Inputs 40bit
+  - One button  8bit
+  - One encoder value 32bit
+ - Outputs 8bit
+  - LED 8bit
+ - Parameters
+  - Encoder settings
+ - Slave commands
+  - Reset counter
+ 
+Now to translate and implement on top of SOES.
+
+First look on the start up code. This example shows how to add a 
+main function that will be called by startup code. In this example 
+main's only purpose is to spawn two new tasks. One that executes 
+SOES and one that control the ERROR LED. Some ESCs provided a pin 
+for the RUN LED, some even for the ERROR LED, if it don't you can 
+control them from the slave Micro Controller. We'll focus on the "soes" task. 
+
+\code
+
+int main (void)
+{
+   rprintp ("SOES (Simple Open EtherCAT Slave)\nsoes test\n");
+
+   /* task_spawn ("led_run", led_error, 15, 512, NULL); /
+   task_spawn ("led_error", led_error, 15, 512, NULL);
+   task_spawn ("soes", soes, 9, 1024, NULL);
+
+   return (0);
+}
+
+\endcode
+
+\section configuration Configuration
+The function soes is our EtherCAT slave device and can be split in 3 parts.
+Hardware Init, Software Init and Application loop. We will start with the
+Hardware Init.
+
+ - Hardware Init
+  - esc_reset, special function used for ESC reset if no Physical 
+  EEPROM used. This is local to the Application not part of the generic
+  ESC handling.
+  - ESC_init, initialise SPI communication or similar
+  - Wait for ESC is started, waiting for SPI to be up and running, 
+  we'll query the ESC register DL status if EEPROM loaded OK and PDI
+  operational, eg. SPI OK.
+  
+\code
+  
+void soes (void *arg)
+{
+   TXPDOsize = SM3_sml = sizeTXPDO ();
+   RXPDOsize = SM2_sml = sizeRXPDO ();
+
+   esc_reset ();
+   ESC_init ((void *)spi_name);
+
+   task_delay (tick_from_ms (200));
+
+   // wait until ESC is started up
+   while ((ESCvar.DLstatus & 0x0001) == 0)
+   {
+      ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
+                sizeof (ESCvar.DLstatus));
+      ESCvar.DLstatus = etohs (ESCvar.DLstatus);
+   }
+
+   // reset ESC to init state
+   ESC_ALstatus (ESCinit);
+...
+}
+
+\endcode
+
+ - Software Init 
+  - Reset the slave into Init state by writing AL Status register.
+  - Clear Errors by writing AL Status Code register.
+  - Stop the Application Layer, basically we disable the SyncManagers 
+  that implicitly block the data exchange.
+
+\code
+  
+void soes (void *arg)
+{
+...
+   while ((ESCvar.DLstatus & 0x0001) == 0)
+   {
+      ESC_read (ESCREG_DLSTATUS, (void *) &ESCvar.DLstatus,
+                sizeof (ESCvar.DLstatus));
+      ESCvar.DLstatus = etohs (ESCvar.DLstatus);
+   }
+
+   // reset ESC to init state
+   ESC_ALstatus (ESCinit);
+   ESC_ALerror (ALERR_NONE);
+   ESC_stopmbx ();
+   ESC_stopinput ();
+   ESC_stopoutput ();
+
+   // application run loop
+   while (1)
+...
+}  
+\endcode
+
+ - Application loop
+  - ALevent handling, ALevent hold information on changes in ALControl or 
+  SyncManagers. ALControl for state changes and SyncManagers for changes written 
+  by EtherCAT in local memory mapped to active SyncManagers
+  - ESC_state for state handling, such as state step up or down with correct 
+  error and acknowledge response.  
+  - Mailbox handler, generic support of mailboxes used by all Application Layer
+  protocols.
+  - On mailbox actions we'll also check if we need to use a specific protocol 
+  handler to handle the incoming or outgoing Mailbox data.
+
+Up until the now we're using the SOES protocol stack without any application specific
+calls. Next up we'll look at the application Code, here named DIG_process ().
+
+\code
+  
+void soes (void *arg)
+{
+...
+   // application run loop
+   while (1)
+   {
+      if((ESCvar.ALstatus & 0x0f) == ESCinit)
+      {
+         txpdomap = DEFAULTTXPDOMAP;
+         rxpdomap = DEFAULTRXPDOMAP;
+         txpdoitems = DEFAULTTXPDOITEMS;
+         rxpdoitems = DEFAULTTXPDOITEMS;
+      }
+      ESC_read (ESCREG_LOCALTIME, (void *) &ESCvar.Time, sizeof (ESCvar.Time));
+      ESCvar.Time = etohl (ESCvar.Time);
+
+      ESC_state ();
+      if (ESC_mbxprocess ())
+      {
+         ESC_coeprocess ();
+         ESC_foeprocess ();
+         ESC_xoeprocess ();
+      }
+      DIG_process ();
+   };
+}
+
+\endcode
+
+\section application Application
+The function DIG_process is the User part of the application and could be joined
+by more cyclic User functions for executing other parts of the application.
+The example code can be split in 2 parts
+ - Outputs
+  - Start by evaluating if we're in a state supporting update of outputs, eg. 
+  Operational state.
+  - If we're in OP we can read the current PDO data in the 3-buffer SyncManager
+  mapped to the output SM, the default is SyncManager2, we read the ESC RAM address
+  of SM2 and store it at the local address of the local variable Wb.LED.
+  We'll read RXPDOsize bytes to trigger a complete SyncManager read. 
+  - After local variables have been refreshed we basically write the local
+  PDO variables to the User application, ex. a GPIO. 
+  - Basically this is the API of the SOES toward the User Application.
+  - This function also include a watchdog mechanism, if triggered it will 
+  shutdown the outputs and trigger a state changes to safe operational. 
+  AlError is updated with cause of error to inform the Master.
+  
+\code
+
+void RXPDO_update (void)
+{
+   ESC_read (SM2_sma, &Wb.LED, RXPDOsize);
+}
+
+void DIG_process (void)
+{
+   if (App.state & APPSTATE_OUTPUT)
+   {
+      if (ESCvar.ALevent & ESCREG_ALEVENT_SM2)  // SM2 trigger ?
+      {
+         RXPDO_update ();
+         reset_wd ();
+         gpio_set(GPIO_LED, Wb.LED & BIT(0));
+
+      }
+      if (!wd_cnt)
+      {
+         ESC_stopoutput ();
+         // watchdog, invalid outputs
+         ESC_ALerror (ALERR_WATCHDOG);
+         // goto safe-op with error bit set
+         ESC_ALstatus (ESCsafeop | ESCerror);
+         wd_trigger = 1;
+      }
+   }
+   else
+   {
+      reset_wd ();
+   }
+...
+}
+
+\endcode
+
+ - Inputs
+  - Is handled a bit simpler than outputs but in reverse order, the slave will 
+  continue update the inputs in state Safe Operational.
+  - Here we first read User application data from ex. GPIO and then write to 
+  local PDO variables.
+  - After the local variables have been refreshed we write those to the current
+  PDO data 3-buffer SyncManager mapped to the input SM, the default is SyncManager3.
+  This way we update the ESC RAM address with User Application data accessed by
+  the EtherCAT master.
+  
+\code
+  
+void TXPDO_update (void)
+{
+   ESC_write (SM3_sma, &Rb.button, TXPDOsize);
+}
+
+void DIG_process (void)
+{
+...
+   Rb.button = gpio_get(GPIO_WAKEUP);
+   Cb.reset_counter++;
+   Rb.encoder =  Cb.reset_counter;
+
+   TXPDO_update ();
+}
+
+\endcode
+
+\section ApplicationdataProcessdata ApplicationdataProcessdata
+To run application data through EtherCAT processdata we need to describe for 
+the fieldbus what data we have and will read/write. For this we have 3 objects, 
+the ESI file, SII-EEPROM and CoE Object Dictionary. The first 2 are mandatory 
+and the third is a very convenient way of describing complex slaves.
+
+Our strategy is to keep the ESI file and the SII-EEPROM as thin as possible to 
+avoid duplication of data that need to be maintained. Both will hold the bare 
+minimum of mandatory + optional data to pass CTT. Optional data will be 
+included to tell EtherCAT that detailed information can be retrieved via CoE 
+from the OD stored in the slave it self.
+
+\section SII-EEPROM SII-EEPROM
+Snapshot from SII information matrix from EtherCAT communication slides.
+ 
+\image html sii_pdo.png "Our target slave is Fixed PDO and OD"
+\image latex sii_pdo.png "Our target slave is Fixed PDO and OD" width=15cm
+
+
+\section ESI-file ESI-file
+Snapshot from ESI tree from EtherCAT communication slides.
+
+\image html esi_pdo.png "mandatory and optional ESI data"
+\image latex esi_pdo.png "mandatory and optional ESI data" width=15cm
+
+To briefly give a hint what are describe in the ESI and SII we're listing 
+a set of included elements marked M for mandatory and O for optional.
+
+\code
+ - Vendor (M) , Describes the identity.
+  - Id (M), Hex, EtherCAT Vendor ID, OD 1018.01
+  - Name (M), NameType, Expedient vendor name
+ - Descriptions (M), Describes the EtherCAT device(s) using elements.
+  - Groups (M), Similar devices can be assigned to one group. 
+   - Group (M), One group groups similar devices with slightly different features
+    - Type (M), A reference handle corresponding to the GroupType value in Description:Devices:Device:Group
+   - Name (M), Name for this group show by a configuration tool
+  - Devices (M), Element devices may describe one or several devices with their EtherCAT features such as SyncManagers, FMMUs and Dictionaries
+   - Device (O), Holds all information about the device like syncmanagers and FMMU, object dictionary, data types and the PDO mapping and assign description
+   - Device ATT: Physics (M),string, Physics at individual ports
+    - Type (M), Device identity 
+   - Type ATT:ProductCode="#x98123467" 
+   - Type ATT:RevisionNo="#x00000001"
+   - Name (M), Detailed name of device shown by a configuration tool (not used for identification)
+    - GroupType (M), Reference to a group (described in element Groups) to which this device should be assigned to. Name of the handle used in element Groups:Group:Type 	 
+    - Fmmu (O), String to describe function, Outputs -> RxPDO, Inputs -> TxPDO , MBoxState -> FMMU is used to poll Input Mailbox
+   - Sm (O), Description of SyncManager including start address and direction. 
+    - MBoxOut Mailbox Data Master -> Slave
+    - MBoxIn Mailbox Data Slave -> Master
+    - Outputs Process Data Master -> Slave
+    - Inputs Process Data Slave -> master  
+   - Sm ATT:DefaultSize="128" , Size
+   - Sm ATT:StartAddress="#x1000" , Start address
+   - Sm ATT:ControlByte="#x26" , Settings , Bit [1][0] = 10, Operation mode Mailbox, 00 Buffered 3.
+   - Sm ATT:Enable="1", Enabled
+   - Mailbox (O), Description of available mailbox protocols
+   - Mailbox ATT: DataLinkLayer="true", Support of Mailbox Data Link Layer is mandatory.
+    - CoE (O), Device support CoE
+    - CoE (O) ATT: SdoInfo="true" , SDO Information Service 
+    - CoE (O) ATT: CompleteAccess="false" , SDO complete access not supported
+    - CoE (O) ATT: PdoUpload="true", PDO description uploaded from the slave's object dictionary and SyncManager length calculated based on the same
+   - Dc (O), describes synchronization modes supported by the device.
+    - OpMode (O), Definition of supported operation  modes
+     - Name (M), Internal Handle of operation mode
+     - Desc (O(M)), description of operation mode, recommended, Free Run (no sync), SM Synchronous, DC Synchronous 
+     - AssignActive (M), Value of Latch and Sync Control registers
+   - Eeprom (O, use is mandatory) 
+    - Data (M)
+	 Or
+    - ByteSize (M), Byte Size of connected EEPROM device
+    - ConfigData (M), First 7 words of EEPROM, Configuration Areas
+    - BootStrap (O), Start address and length of mailboxes for BootStrap
+\endcode
+
+So to describe the application we use CoE and Object Dictionary. The mapping between
+Object Dictionary and the User Application are done via local variables defined as 
+user types. The Object Dictionary itself is stored in a matrix where all 
+the indexes are listed. Every index then have a submatrix with its subindex.
+
+\section ObjectDictionary ObjectDictionary
+The Object Dictionary used as example follow the CANopen DS301 ranges. 
+
+ - 0x0000 - 0x0FFF, Data Type Area
+ - 0x1000 - 0x1FFF, Communication Area
+ - 0x2000 - 0x5FFF, Manufacture specific area
+ - 0x6000 - 0x6FFF, Input area
+ - 0x7000 - 0x7FFF, Output area
+ - 0x8000 - 0x8FFF, Configuration area
+ - 0x9000 - 0x9FFF, Information area
+ - 0xA000 - 0xAFFF, Diagnosis Area
+ - 0xB000 - 0xBFFF, Service Transfer Area
+ - 0xC000 - 0xEFFF, Reserved Area
+ - 0xF000 - 0xFFFF, Device Area
+
+RxPDO , 0x1600 - 0x17FF
+TxPDO , 0x1A00 - 0x1BFF
+
+Example, on how the the OD index are linked.
+Top index, SyncManagers Communication Types. In index 0 the 0x04 indicates we have 4 
+SyncManagers defined. Every SyncManager is assigned a type, in index 1-4, we have standard
+settings SM0 = 1, SM1 = 2, SM2 = 3, SM3 = 4 from ETG 1000.6, 5.6.7.4.  
+ - 0, Unused
+ - 1, MailBox Receive, master to slave 
+ - 2, MailBox Send, slave to master 
+ - 3, Processdata output, master to slave
+ - 4, Processdata input, slave to master
+
+\code
+objectlist.h
+FLASHSTORE _objectlist SDOobjects[] =
+...
+  {0x1C00, OTYPE_ARRAY, 4, 0, &acName1C00[0], &SDO1C00[0]},
+  
+FLASHSTORE _objd SDO1C00[] =
+{ {0x00, DTYPE_UNSIGNED8, 8, ATYPE_R, &acNameNOE[0], 0x04, nil},
+  {0x01, DTYPE_UNSIGNED8, 8, ATYPE_R, &acName1C00_01[0], 0x01, nil},
+  {0x02, DTYPE_UNSIGNED8, 8, ATYPE_R, &acName1C00_02[0], 0x02, nil},
+  {0x03, DTYPE_UNSIGNED8, 8, ATYPE_R, &acName1C00_03[0], 0x03, nil},
+  {0x04, DTYPE_UNSIGNED8, 8, ATYPE_R, &acName1C00_04[0], 0x04, nil}
+  
+\endcode
+
+SyncManagers channels 0-31 are listed in SDO1C10-SDO1C2F. If we look
+at SyncManager channel 2, we see.
+ 
+ - Type 3, Processdata output, master to slave
+
+It got one RxPDO index 0x1600 connected, and the submatrix for 0x1600 link
+one PDO object index 0x7000 subindex 1. The output object 0x70000108 give 
+you the index 0x7000, subindex 1 and PDO object length 1(byte).
+
+\code
+objectlist.h
+FLASHSTORE _objd SDO1C12[] =
+{ {0x00, DTYPE_UNSIGNED8, 8, ATYPE_R, &acNameNOE[0], 0x01, nil},
+  {0x01, DTYPE_UNSIGNED16, 16, ATYPE_R, &acNameMO[0], 0x1600, nil}
+};
+  
+FLASHSTORE _objd SDO1600[] =
+{ {0x00, DTYPE_UNSIGNED8, 8, ATYPE_R, &acNameNOE[0], 0x01, nil},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_R, &acNameMO[0], 0x70000108, nil}
+};
+  
+\endcode
+
+At PDO level we make the connection between the local application and
+the object dictionary. For all subindex in the PDO the last element
+is the address to the local variable.
+
+\code
+
+objectlist.h
+FLASHSTORE _objd SDO7000[] =
+{ {0x00, DTYPE_UNSIGNED8, 8, ATYPE_R, &acNameNOE[0], 0x01, nil},
+  {0x01, DTYPE_UNSIGNED8, 8, ATYPE_RW, &acName7000_01[0], 0, &(Wb.LED)}
+};
+
+utypes.h
+typedef struct
+{
+   uint8 LED;
+} _Wbuffer;
+  
+\endcode
+
+Beside SyncManager to PDO mapping we also have mandatory data as 
+
+0x1000 Device Type
+0x1018 Object Identity
+0x10C0 SyncManager Communication Type, as we used as top index when 
+figuring out our PDOs in the Object Dictionary.
+
+For a complete description of the object dictionary you can get guidance
+by the ETG1000.6 EcatAlProtocols
+
+A useful feature is the Asynchronous use of SDO parameters. In the example
+we have Parameter set holding an encoder scale value, just for show we also
+have a read only mirror value of the encoder scale. Parameters defined as a
+RW SDO parameter and can be written/read by SDO Download or Upload. In 
+addition there is a ESC_objecthandler Hook on SDO Download where you can 
+add additional logic, ex. we execute the mirror of the encoder scale value
+by assigning the encoder scale value to the mirror.
+
+\code
+
+objectlist.h
+FLASHSTORE _objd SDO7100[] =
+{ {0x00, DTYPE_UNSIGNED8, 8, ATYPE_R, &acNameNOE[0], 0x02, nil},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RW, &acName7100_01[0], 0, &(encoder_scale)},
+  {0x02, DTYPE_UNSIGNED32, 32, ATYPE_R, &acName7100_02[0], 0, &(encoder_scale_mirror)}
+};
+
+soes.c
+void ESC_objecthandler (uint16 index, uint8 subindex)
+{
+   switch (index)
+   {
+...
+      case 0x7100:
+      {
+         switch (subindex)
+         {
+            case 0x01:
+            {
+               encoder_scale_mirror = encoder_scale;
+               break;
+            }
+         }
+         break;
+      }
+...
+}
+
+\endcode
+This tutorial is just one way of doing it.
+Enjoy and happy coding!
+ 
+Andreas Karlsson, rt-labs AB, www.rt-labs.com
+ */

Cards/EaserCAT-2000/Firmware/lib/soes/ecat_options.h

@@ -0,0 +1,42 @@
+#ifndef __ECAT_OPTIONS_H__
+#define __ECAT_OPTIONS_H__
+
+#define USE_FOE          0
+#define USE_EOE          0
+
+#define MBXSIZE          512
+#define MBXSIZEBOOT      512
+#define MBXBUFFERS       3
+
+#define MBX0_sma         0x1000
+#define MBX0_sml         MBXSIZE
+#define MBX0_sme         MBX0_sma+MBX0_sml-1
+#define MBX0_smc         0x26
+#define MBX1_sma         MBX0_sma+MBX0_sml
+#define MBX1_sml         MBXSIZE
+#define MBX1_sme         MBX1_sma+MBX1_sml-1
+#define MBX1_smc         0x22
+
+#define MBX0_sma_b       0x1000
+#define MBX0_sml_b       MBXSIZEBOOT
+#define MBX0_sme_b       MBX0_sma_b+MBX0_sml_b-1
+#define MBX0_smc_b       0x26
+#define MBX1_sma_b       MBX0_sma_b+MBX0_sml_b
+#define MBX1_sml_b       MBXSIZEBOOT
+#define MBX1_sme_b       MBX1_sma_b+MBX1_sml_b-1
+#define MBX1_smc_b       0x22
+
+#define SM2_sma          0x1600
+#define SM2_smc          0x24
+#define SM2_act          1
+#define SM3_sma          0x1A00
+#define SM3_smc          0x20
+#define SM3_act          1
+
+#define MAX_MAPPINGS_SM2 5
+#define MAX_MAPPINGS_SM3 11
+
+#define MAX_RXPDO_SIZE   512
+#define MAX_TXPDO_SIZE   512
+
+#endif /* __ECAT_OPTIONS_H__ */

Cards/EaserCAT-2000/Firmware/lib/soes/ecat_slv.c

@@ -0,0 +1,386 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+#include <stddef.h>
+#include "esc.h"
+#include "esc_coe.h"
+#include "esc_foe.h"
+#include "esc_eoe.h"
+#include "ecat_slv.h"
+
+#define IS_RXPDO(index) ((index) >= 0x1600 && (index) < 0x1800)
+#define IS_TXPDO(index) ((index) >= 0x1A00 && (index) < 0x1C00)
+
+/* Global variables used by the stack */
+uint8_t MBX[MBXBUFFERS * MAX(MBXSIZE, MBXSIZEBOOT)];
+_MBXcontrol MBXcontrol[MBXBUFFERS];
+_SMmap SMmap2[MAX_MAPPINGS_SM2];
+_SMmap SMmap3[MAX_MAPPINGS_SM3];
+_ESCvar ESCvar;
+
+/* Private variables */
+static volatile int watchdog;
+
+#if MAX_MAPPINGS_SM2 > 0
+static uint8_t rxpdo[MAX_RXPDO_SIZE] __attribute__((aligned(8)));
+#else
+extern uint8_t *rxpdo;
+#endif
+
+#if MAX_MAPPINGS_SM3 > 0
+static uint8_t txpdo[MAX_TXPDO_SIZE] __attribute__((aligned(8)));
+#else
+extern uint8_t *txpdo;
+#endif
+
+/** Function to pre-qualify the incoming SDO download.
+ *
+ * @param[in] index      = index of SDO download request to check
+ * @param[in] sub-index  = sub-index of SDO download request to check
+ * @return SDO abort code, or 0 on success
+ */
+uint32_t ESC_download_pre_objecthandler(uint16_t index,
+                                        uint8_t subindex,
+                                        void *data,
+                                        size_t size,
+                                        uint16_t flags)
+{
+   if (IS_RXPDO(index) ||
+       IS_TXPDO(index) ||
+       index == RX_PDO_OBJIDX ||
+       index == TX_PDO_OBJIDX)
+   {
+      uint8_t minSub = ((flags & COMPLETE_ACCESS_FLAG) == 0) ? 0 : 1;
+      if (subindex > minSub && COE_maxSub(index) != 0)
+      {
+         return ABORT_SUBINDEX0_NOT_ZERO;
+      }
+   }
+
+   if (ESCvar.pre_object_download_hook)
+   {
+      return (ESCvar.pre_object_download_hook)(index,
+                                               subindex,
+                                               data,
+                                               size,
+                                               flags);
+   }
+
+   return 0;
+}
+
+/** Hook called from the slave stack SDO Download handler to act on
+ * user specified Index and Sub-index.
+ *
+ * @param[in] index      = index of SDO download request to handle
+ * @param[in] sub-index  = sub-index of SDO download request to handle
+ * @return SDO abort code, or 0 on success
+ */
+uint32_t ESC_download_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
+{
+   if (ESCvar.post_object_download_hook != NULL)
+   {
+      return (ESCvar.post_object_download_hook)(index, subindex, flags);
+   }
+
+   return 0;
+}
+
+/** Function to pre-qualify the incoming SDO upload.
+ *
+ * @param[in] index      = index of SDO upload request to handle
+ * @param[in] sub-index  = sub-index of SDO upload request to handle
+ * @return SDO abort code, or 0 on success
+ */
+uint32_t ESC_upload_pre_objecthandler(uint16_t index,
+                                      uint8_t subindex,
+                                      void *data,
+                                      size_t size,
+                                      uint16_t flags)
+{
+   if (ESCvar.pre_object_upload_hook != NULL)
+   {
+      return (ESCvar.pre_object_upload_hook)(index,
+                                             subindex,
+                                             data,
+                                             size,
+                                             flags);
+   }
+
+   return 0;
+}
+
+/** Hook called from the slave stack SDO Upload handler to act on
+ * user specified Index and Sub-index.
+ *
+ * @param[in] index      = index of SDO upload request to handle
+ * @param[in] sub-index  = sub-index of SDO upload request to handle
+ * @return SDO abort code, or 0 on success
+ */
+uint32_t ESC_upload_post_objecthandler(uint16_t index, uint8_t subindex, uint16_t flags)
+{
+   if (ESCvar.post_object_upload_hook != NULL)
+   {
+      return (ESCvar.post_object_upload_hook)(index, subindex, flags);
+   }
+
+   return 0;
+}
+
+/** Hook called from the slave stack ESC_stopoutputs to act on state changes
+ * forcing us to stop outputs. Here we can set them to a safe state.
+ */
+void APP_safeoutput(void)
+{
+   DPRINT("APP_safeoutput\n");
+
+   if (ESCvar.safeoutput_override != NULL)
+   {
+      (ESCvar.safeoutput_override)();
+   }
+}
+
+/** Write local process data to Sync Manager 3, Master Inputs.
+ */
+void TXPDO_update(void)
+{
+   if (ESCvar.txpdo_override != NULL)
+   {
+      (ESCvar.txpdo_override)();
+   }
+   else
+   {
+      if (MAX_MAPPINGS_SM3 > 0)
+      {
+         COE_pdoPack(txpdo, ESCvar.sm3mappings, SMmap3);
+      }
+      ESC_write(ESC_SM3_sma, txpdo, ESCvar.ESC_SM3_sml);
+   }
+}
+
+/** Read Sync Manager 2 to local process data, Master Outputs.
+ */
+void RXPDO_update(void)
+{
+   if (ESCvar.rxpdo_override != NULL)
+   {
+      (ESCvar.rxpdo_override)();
+   }
+   else
+   {
+      ESC_read(ESC_SM2_sma, rxpdo, ESCvar.ESC_SM2_sml);
+      if (MAX_MAPPINGS_SM2 > 0)
+      {
+         COE_pdoUnpack(rxpdo, ESCvar.sm2mappings, SMmap2);
+      }
+   }
+}
+
+/* Set the watchdog count value, don't have any affect when using
+ * HW watchdog 0x4xx
+ *
+ * @param[in] watchdogcnt  = new watchdog count value
+ */
+void APP_setwatchdog(int watchdogcnt)
+{
+   CC_ATOMIC_SET(ESCvar.watchdogcnt, watchdogcnt);
+}
+
+/* Function to update local I/O, call read ethercat outputs, call
+ * write ethercat inputs. Implement watch-dog counter to count-out if we have
+ * made state change affecting the App.state.
+ */
+
+void DIG_process(uint16_t ALEvent, uint8_t flags)
+{
+   /* Handle watchdog */
+   if ((flags & DIG_PROCESS_WD_FLAG) > 0)
+   {
+      if (CC_ATOMIC_GET(watchdog) > 0)
+      {
+         CC_ATOMIC_SUB(watchdog, 1);
+      }
+
+      if ((CC_ATOMIC_GET(watchdog) <= 0) &&
+          ((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0))
+      {
+         DPRINT("DIG_process watchdog expired\n");
+         ESC_ALstatusgotoerror((ESCsafeop | ESCerror), ALERR_WATCHDOG);
+      }
+      else if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) == 0))
+      {
+         CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
+      }
+   }
+
+   /* Handle Outputs */
+   if ((flags & DIG_PROCESS_OUTPUTS_FLAG) > 0)
+   {
+      if (((CC_ATOMIC_GET(ESCvar.App.state) & APPSTATE_OUTPUT) > 0) &&
+          (ALEvent & ESCREG_ALEVENT_SM2))
+      {
+         RXPDO_update();
+         CC_ATOMIC_SET(watchdog, ESCvar.watchdogcnt);
+         /* Set outputs */
+         cb_set_outputs();
+      }
+      else if (ALEvent & ESCREG_ALEVENT_SM2)
+      {
+         RXPDO_update();
+      }
+   }
+
+   /* Call application */
+   if ((flags & DIG_PROCESS_APP_HOOK_FLAG) > 0)
+   {
+      /* Call application callback if set */
+      if (ESCvar.application_hook != NULL)
+      {
+         (ESCvar.application_hook)();
+      }
+   }
+
+   /* Handle Inputs */
+   if ((flags & DIG_PROCESS_INPUTS_FLAG) > 0)
+   {
+      if (CC_ATOMIC_GET(ESCvar.App.state) > 0)
+      {
+         /* Update inputs */
+         cb_get_inputs();
+         TXPDO_update();
+      }
+   }
+}
+
+/*
+ * Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
+ * control what interrupts that should be served and re-activated with
+ * event mask argument
+ */
+void ecat_slv_worker(uint32_t event_mask)
+{
+   do
+   {
+      /* Check the state machine */
+      ESC_state();
+      /* Check the SM activation event */
+      ESC_sm_act_event();
+
+      /* Check mailboxes */
+      while ((ESC_mbxprocess() > 0) || (ESCvar.txcue > 0))
+      {
+         ESC_coeprocess();
+#if USE_FOE
+         ESC_foeprocess();
+#endif
+#if USE_EOE
+         ESC_eoeprocess();
+#endif
+         ESC_xoeprocess();
+      }
+#if USE_EOE
+      ESC_eoeprocess_tx();
+#endif
+      /* Call emulated eeprom handler if set */
+      if (ESCvar.esc_hw_eep_handler != NULL)
+      {
+         (ESCvar.esc_hw_eep_handler)();
+      }
+
+      CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread());
+
+   } while (ESCvar.ALevent & event_mask);
+
+   ESC_ALeventmaskwrite(ESC_ALeventmaskread() | event_mask);
+}
+
+/*
+ * Polling function. It should be called periodically for an application
+ * when only SM2/DC interrupt is active.
+ * Read and handle events for the EtherCAT state, status, mailbox and eeprom.
+ */
+void ecat_slv_poll(void)
+{
+   /* Read local time from ESC*/
+   ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time));
+   ESCvar.Time = etohl(ESCvar.Time);
+
+   /* Check the state machine */
+   ESC_state();
+   /* Check the SM activation event */
+   ESC_sm_act_event();
+
+   /* Check mailboxes */
+   if (ESC_mbxprocess())
+   {
+      ESC_coeprocess();
+#if USE_FOE
+      ESC_foeprocess();
+#endif
+#if USE_EOE
+      ESC_eoeprocess();
+#endif
+      ESC_xoeprocess();
+   }
+#if USE_EOE
+   ESC_eoeprocess_tx();
+#endif
+
+   /* Call emulated eeprom handler if set */
+   if (ESCvar.esc_hw_eep_handler != NULL)
+   {
+      (ESCvar.esc_hw_eep_handler)();
+   }
+}
+
+/*
+ * Poll all events in a free-run application
+ */
+void ecat_slv(void)
+{
+   ecat_slv_poll();
+   DIG_process(ESC_ALeventread(), DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+                                      DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
+}
+
+/*
+ * Initialize the slave stack.
+ */
+void ecat_slv_init(esc_cfg_t *config)
+{
+   /* Init watchdog */
+   watchdog = config->watchdog_cnt;
+
+   /* Call stack configuration */
+   ESC_config(config);
+   /* Call HW init */
+   ESC_init(config);
+
+   /*  wait until ESC is started up */
+   while ((ESCvar.DLstatus & 0x0001) == 0)
+   {
+      ESC_read(ESCREG_DLSTATUS, (void *)&ESCvar.DLstatus,
+               sizeof(ESCvar.DLstatus));
+      ESCvar.DLstatus = etohs(ESCvar.DLstatus);
+   }
+
+#if USE_FOE
+   /* Init FoE */
+   FOE_init();
+#endif
+
+#if USE_EOE
+   /* Init EoE */
+   EOE_init();
+#endif
+
+   /* reset ESC to init state */
+   ESC_ALstatus(ESCinit);
+   ESC_ALerror(ALERR_NONE);
+   ESC_stopmbx();
+   ESC_stopinput();
+   ESC_stopoutput();
+   /* Init Object Dictionary default values */
+   COE_initDefaultValues();
+}

Cards/EaserCAT-2000/Firmware/lib/soes/ecat_slv.h

@@ -0,0 +1,69 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+#ifndef __ECAT_SLV_H__
+#define __ECAT_SLV_H__
+
+#include "ecat_options.h"
+#include "esc.h"
+
+/**
+ * This function is called when to get input values
+ */
+void cb_get_inputs();
+
+/**
+ * This function is called when to set outputs values
+ */
+void cb_set_outputs();
+
+/** Set the watchdog count value
+ *
+ * @param[in] watchdogcnt  = new watchdog count value
+ */
+void APP_setwatchdog(int watchdogcnt);
+
+#define DIG_PROCESS_INPUTS_FLAG 0x01
+#define DIG_PROCESS_OUTPUTS_FLAG 0x02
+#define DIG_PROCESS_WD_FLAG 0x04
+#define DIG_PROCESS_APP_HOOK_FLAG 0x08
+/** Implements the watch-dog counter to count if we should make a state change
+ * due to missing incoming SM2 events. Updates local I/O and run the application
+ * in the following order, call read EtherCAT outputs, execute user provided
+ * application hook and call write EtherCAT inputs.
+ *
+ * @param[in]   flags     = User input what to execute
+ */
+void DIG_process(uint16_t ALEvent, uint8_t flags);
+
+/**
+ * Handler for SM change, SM0/1, AL CONTROL and EEPROM events, the application
+ * control what interrupts that should be served and re-activated with
+ * event mask argument
+ *
+ * @param[in]   event_mask = Event mask for interrupts to serve and re-activate
+ *                           after served
+ */
+void ecat_slv_worker(uint32_t event_mask);
+
+/**
+ * Poll SM0/1, EEPROM and AL CONTROL events in a SM/DC synchronization
+ * application
+ */
+void ecat_slv_poll(void);
+
+/**
+ * Poll all events in a free-run application
+ */
+void ecat_slv(void);
+
+/**
+ * Initialize the slave stack
+ *
+ * @param[in]   config     = User input how to configure the stack
+ */
+void ecat_slv_init(esc_cfg_t *config);
+
+#endif /* __ECAT_SLV_H__ */

Cards/EaserCAT-2000/Firmware/lib/soes/eeprom.hex

@@ -0,0 +1,65 @@
+:2000000080060344640000000000000000001400AA0A0000CCBCBB000200000001000000A1
+:20002000000000000000000000000000000000000010000200120002040000000000000096
+:200040000000000000000000000000000000000000000000000000000000000000000000A0
+:20006000000000000000000000000000000000000000000000000000000000000F00010070
+:200080000A002000040C4561736572434154323030300E4D616368696E65436F6E74726F64
+:2000A0006C06494D474342591A4D6574616C4D7573696E67732045617365724341542032E6
+:2000C000303030001E00100002030104001300000000000000000000110000000000000034
+:2000E000000000000000000028000200010203002900100000100002260001010012000249
+:20010000220001020016000024000103001A000020000104FFFFFFFFFFFFFFFFFFFFFFFF49
+:20012000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDF
+:20014000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBF
+:20016000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9F
+:20018000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7F
+:2001A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5F
+:2001C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3F
+:2001E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1F
+:20020000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFE
+:20022000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDE
+:20024000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBE
+:20026000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9E
+:20028000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7E
+:2002A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5E
+:2002C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3E
+:2002E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1E
+:20030000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD
+:20032000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDD
+:20034000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBD
+:20036000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9D
+:20038000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7D
+:2003A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5D
+:2003C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3D
+:2003E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1D
+:20040000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFC
+:20042000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDC
+:20044000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBC
+:20046000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9C
+:20048000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7C
+:2004A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5C
+:2004C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3C
+:2004E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1C
+:20050000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB
+:20052000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDB
+:20054000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBB
+:20056000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9B
+:20058000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7B
+:2005A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5B
+:2005C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3B
+:2005E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1B
+:20060000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFA
+:20062000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFDA
+:20064000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFBA
+:20066000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9A
+:20068000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF7A
+:2006A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF5A
+:2006C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF3A
+:2006E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF1A
+:20070000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF9
+:20072000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFD9
+:20074000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFB9
+:20076000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF99
+:20078000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF79
+:2007A000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF59
+:2007C000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF39
+:2007E000FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF19
+:00000001FF

Cards/EaserCAT-2000/Firmware/lib/soes/esc.h

@@ -0,0 +1,769 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+/** \file
+ * \brief
+ * Headerfile for esc.h
+ */
+
+#ifndef __esc__
+#define __esc__
+
+#include <stdbool.h>
+#include <cc.h>
+#include <esc_coe.h>
+#include "ecat_options.h"
+
+#define ESCREG_ADDRESS              0x0010
+#define ESCREG_DLSTATUS             0x0110
+#define ESCREG_ALCONTROL            0x0120
+#define ESCREG_ALSTATUS             0x0130
+#define ESCREG_ALERROR              0x0134
+#define ESCREG_ALEVENTMASK          0x0204
+#define ESCREG_ALEVENT              0x0220
+#define ESCREG_ALEVENT_SM_MASK      0x0310
+#define ESCREG_ALEVENT_SMCHANGE     0x0010
+#define ESCREG_ALEVENT_CONTROL      0x0001
+#define ESCREG_ALEVENT_DC_LATCH     0x0002
+#define ESCREG_ALEVENT_DC_SYNC0     0x0004
+#define ESCREG_ALEVENT_DC_SYNC1     0x0008
+#define ESCREG_ALEVENT_EEP          0x0020
+#define ESCREG_ALEVENT_WD           0x0040
+#define ESCREG_ALEVENT_SM0          0x0100
+#define ESCREG_ALEVENT_SM1          0x0200
+#define ESCREG_ALEVENT_SM2          0x0400
+#define ESCREG_ALEVENT_SM3          0x0800
+#define ESCREG_WDSTATUS             0x0440
+#define ESCREG_EECONTSTAT           0x0502
+#define ESCREG_EEDATA               0x0508
+#define ESCREG_SM0                  0x0800
+#define ESCREG_SM0STATUS            (ESCREG_SM0 + 5)
+#define ESCREG_SM0ACTIVATE          (ESCREG_SM0 + 6)
+#define ESCREG_SM0PDI               (ESCREG_SM0 + 7)
+#define ESCREG_SM1                  (ESCREG_SM0 + 0x08)
+#define ESCREG_SM2                  (ESCREG_SM0 + 0x10)
+#define ESCREG_SM3                  (ESCREG_SM0 + 0x18)
+#define ESCREG_LOCALTIME            0x0910
+#define ESCREG_LOCALTIME_OFFSET     0x0920
+#define ESCREG_SYNC_ACT             0x0981
+#define ESCREG_SYNC_ACT_ACTIVATED   0x01
+#define ESCREG_SYNC_SYNC0_EN        0x02
+#define ESCREG_SYNC_SYNC1_EN        0x04
+#define ESCREG_SYNC_AUTO_ACTIVATED  0x08
+#define ESCREG_SYNC0_CYCLE_TIME     0x09A0
+#define ESCREG_SYNC1_CYCLE_TIME     0x09A4
+#define ESCREG_SMENABLE_BIT         0x01
+#define ESCREG_AL_STATEMASK         0x001f
+#define ESCREG_AL_ALLBUTINITMASK    0x0e
+#define ESCREG_AL_ERRACKMASK        0x0f
+
+#define SYNCTYPE_SUPPORT_FREERUN    0x01
+#define SYNCTYPE_SUPPORT_SYNCHRON   0x02
+#define SYNCTYPE_SUPPORT_DCSYNC0    0x04
+#define SYNCTYPE_SUPPORT_DCSYNC1    0x08
+#define SYNCTYPE_SUPPORT_SUBCYCLE   0x10
+
+#define ESCinit                  0x01
+#define ESCpreop                 0x02
+#define ESCboot                  0x03
+#define ESCsafeop                0x04
+#define ESCop                    0x08
+#define ESCerror                 0x10
+
+#define INIT_TO_INIT             0x11
+#define INIT_TO_PREOP            0x21
+#define INIT_TO_BOOT             0x31
+#define INIT_TO_SAFEOP           0x41
+#define INIT_TO_OP               0x81
+#define PREOP_TO_INIT            0x12
+#define PREOP_TO_PREOP           0x22
+#define PREOP_TO_BOOT            0x32
+#define PREOP_TO_SAFEOP          0x42
+#define PREOP_TO_OP              0x82
+#define BOOT_TO_INIT             0x13
+#define BOOT_TO_PREOP            0x23
+#define BOOT_TO_BOOT             0x33
+#define BOOT_TO_SAFEOP           0x43
+#define BOOT_TO_OP               0x83
+#define SAFEOP_TO_INIT           0x14
+#define SAFEOP_TO_PREOP          0x24
+#define SAFEOP_TO_BOOT           0x34
+#define SAFEOP_TO_SAFEOP         0x44
+#define SAFEOP_TO_OP             0x84
+#define OP_TO_INIT               0x18
+#define OP_TO_PREOP              0x28
+#define OP_TO_BOOT               0x38
+#define OP_TO_SAFEOP             0x48
+#define OP_TO_OP                 0x88
+
+#define ALERR_NONE                  0x0000
+#define ALERR_UNSPECIFIEDERROR      0x0001
+#define ALERR_NOMEMORY              0x0002
+#define ALERR_INVALIDSTATECHANGE    0x0011
+#define ALERR_UNKNOWNSTATE          0x0012
+#define ALERR_BOOTNOTSUPPORTED      0x0013
+#define ALERR_NOVALIDFIRMWARE       0x0014
+#define ALERR_INVALIDBOOTMBXCONFIG  0x0015
+#define ALERR_INVALIDMBXCONFIG      0x0016
+#define ALERR_INVALIDSMCONFIG       0x0017
+#define ALERR_NOVALIDINPUTS         0x0018
+#define ALERR_NOVALIDOUTPUTS        0x0019
+#define ALERR_SYNCERROR             0x001A
+#define ALERR_WATCHDOG              0x001B
+#define ALERR_INVALIDSYNCMANAGERTYP 0x001C
+#define ALERR_INVALIDOUTPUTSM       0x001D
+#define ALERR_INVALIDINPUTSM        0x001E
+#define ALERR_INVALIDWDTCFG         0x001F
+#define ALERR_SLAVENEEDSCOLDSTART   0x0020
+#define ALERR_SLAVENEEDSINIT        0x0021
+#define ALERR_SLAVENEEDSPREOP       0x0022
+#define ALERR_SLAVENEEDSSAFEOP      0x0023
+#define ALERR_INVALIDINPUTMAPPING   0x0024
+#define ALERR_INVALIDOUTPUTMAPPING  0x0025
+#define ALERR_INCONSISTENTSETTINGS  0x0026
+#define ALERR_FREERUNNOTSUPPORTED   0x0027
+#define ALERR_SYNCNOTSUPPORTED      0x0028
+#define ALERR_FREERUNNEEDS3BUFFMODE 0x0029
+#define ALERR_BACKGROUNDWATCHDOG    0x002A
+#define ALERR_NOVALIDINPUTSOUTPUTS  0x002B
+#define ALERR_FATALSYNCERROR        0x002C
+#define ALERR_NOSYNCERROR           0x002D
+#define ALERR_INVALIDINPUTFMMUCFG   0x002E
+#define ALERR_DCINVALIDSYNCCFG      0x0030
+#define ALERR_INVALIDDCLATCHCFG     0x0031
+#define ALERR_PLLERROR              0x0032
+#define ALERR_DCSYNCIOERROR         0x0033
+#define ALERR_DCSYNCTIMEOUT         0x0034
+#define ALERR_DCSYNCCYCLETIME       0x0035
+#define ALERR_DCSYNC0CYCLETIME      0x0036
+#define ALERR_DCSYNC1CYCLETIME      0x0037
+#define ALERR_MBXAOE                0x0041
+#define ALERR_MBXEOE                0x0042
+#define ALERR_MBXCOE                0x0043
+#define ALERR_MBXFOE                0x0044
+#define ALERR_MBXSOE                0x0045
+#define ALERR_MBXVOE                0x004F
+#define ALERR_EEPROMNOACCESS        0x0050
+#define ALERR_EEPROMERROR           0x0051
+#define ALERR_SLAVERESTARTEDLOCALLY 0x0060
+#define ALERR_DEVICEIDVALUEUPDATED  0x0061
+#define ALERR_APPLCTRLAVAILABLE     0x00f0
+#define ALERR_UNKNOWN               0xffff
+
+#define MBXERR_SYNTAX                   0x0001
+#define MBXERR_UNSUPPORTEDPROTOCOL      0x0002
+#define MBXERR_INVALIDCHANNEL           0x0003
+#define MBXERR_SERVICENOTSUPPORTED      0x0004
+#define MBXERR_INVALIDHEADER            0x0005
+#define MBXERR_SIZETOOSHORT             0x0006
+#define MBXERR_NOMOREMEMORY             0x0007
+#define MBXERR_INVALIDSIZE              0x0008
+
+#define ABORT_NOTOGGLE                  0x05030000
+#define ABORT_TRANSFER_TIMEOUT          0x05040000
+#define ABORT_UNKNOWN                   0x05040001
+#define ABORT_INVALID_BLOCK_SIZE        0x05040002
+#define ABORT_INVALID_SEQUENCE_NUMBER   0x05040003
+#define ABORT_BLOCK_CRC_ERROR           0x05040004
+#define ABORT_OUT_OF_MEMORY             0x05040005
+#define ABORT_UNSUPPORTED               0x06010000
+#define ABORT_WRITEONLY                 0x06010001
+#define ABORT_READONLY                  0x06010002
+#define ABORT_SUBINDEX0_NOT_ZERO        0x06010003
+#define ABORT_CA_NOT_SUPPORTED          0x06010004
+#define ABORT_EXCEEDS_MBOX_SIZE         0x06010005
+#define ABORT_SDO_DOWNLOAD_BLOCKED      0x06010006
+#define ABORT_NOOBJECT                  0x06020000
+#define ABORT_MAPPING_OBJECT_ERROR      0x06040041
+#define ABORT_MAPPING_LENGTH_ERROR      0x06040042
+#define ABORT_GENERAL_PARAMETER_ERROR   0x06040043
+#define ABORT_GENERAL_DEVICE_ERROR      0x06040047
+#define ABORT_HARDWARE_ERROR            0x06060000
+#define ABORT_TYPEMISMATCH              0x06070010
+#define ABORT_DATATYPE_TOO_HIGH         0x06070012
+#define ABORT_DATATYPE_TOO_LOW          0x06070013
+#define ABORT_NOSUBINDEX                0x06090011
+#define ABORT_VALUE_EXCEEDED            0x06090030
+#define ABORT_VALUE_TOO_HIGH            0x06090031
+#define ABORT_VALUE_TOO_LOW             0x06090032
+#define ABORT_MODULE_LIST_MISMATCH      0x06090033
+#define ABORT_MAX_VAL_LESS_THAN_MIN_VAL 0x06090036
+#define ABORT_RESOURCE_NOT_AVAILABLE    0x060A0023
+#define ABORT_GENERALERROR              0x08000000
+#define ABORT_DATA_STORE_ERROR          0x08000020
+#define ABORT_DATA_STORE_LOCAL_ERROR    0x08000021
+#define ABORT_NOTINTHISSTATE            0x08000022
+#define ABORT_OBJECT_DICTIONARY_ERROR   0x08000023
+#define ABORT_NO_DATA_AVAILABLE         0x08000024
+
+#define MBXstate_idle                   0x00
+#define MBXstate_inclaim                0x01
+#define MBXstate_outclaim               0x02
+#define MBXstate_outreq                 0x03
+#define MBXstate_outpost                0x04
+#define MBXstate_backup                 0x05
+#define MBXstate_again                  0x06
+
+#define COE_DEFAULTLENGTH               0x0a
+#define COE_HEADERSIZE                  0x0a
+#define COE_SEGMENTHEADERSIZE           0x03
+#define COE_SDOREQUEST                  0x02
+#define COE_SDORESPONSE                 0x03
+#define COE_SDOINFORMATION              0x08
+#define COE_COMMAND_SDOABORT            0x80
+#define COE_COMMAND_UPLOADREQUEST       0x40
+#define COE_COMMAND_UPLOADRESPONSE      0x40
+#define COE_COMMAND_UPLOADSEGMENT       0x00
+#define COE_COMMAND_UPLOADSEGREQ        0x60
+#define COE_COMMAND_DOWNLOADREQUEST     0x20
+#define COE_COMMAND_DOWNLOADRESPONSE    0x60
+#define COE_COMMAND_DOWNLOADSEGREQ      0x00
+#define COE_COMMAND_DOWNLOADSEGRESP     0x20
+#define COE_COMMAND_LASTSEGMENTBIT      0x01
+#define COE_SIZE_INDICATOR              0x01
+#define COE_EXPEDITED_INDICATOR         0x02
+#define COE_COMPLETEACCESS              0x10
+#define COE_TOGGLEBIT                   0x10
+#define COE_INFOERROR                   0x07
+#define COE_GETODLISTRESPONSE           0x02
+#define COE_GETODRESPONSE               0x04
+#define COE_ENTRYDESCRIPTIONRESPONSE    0x06
+#define COE_VALUEINFO_ACCESS            0x01
+#define COE_VALUEINFO_OBJECT            0x02
+#define COE_VALUEINFO_MAPPABLE          0x04
+#define COE_VALUEINFO_TYPE              0x08
+#define COE_VALUEINFO_DEFAULT           0x10
+#define COE_VALUEINFO_MINIMUM           0x20
+#define COE_VALUEINFO_MAXIMUM           0x40
+#define COE_MINIMUM_LENGTH              8
+
+#define MBXERR                         0x00
+#define MBXAOE                         0x01
+#define MBXEOE                         0x02
+#define MBXCOE                         0x03
+#define MBXFOE                         0x04
+#define MBXODL                         0x10
+#define MBXOD                          0x20
+#define MBXED                          0x30
+#define MBXSEU                         0x40
+#define MBXSED                         0x50
+
+#define SMRESULT_ERRSM0                0x01
+#define SMRESULT_ERRSM1                0x02
+#define SMRESULT_ERRSM2                0x04
+#define SMRESULT_ERRSM3                0x08
+
+#define FOE_ERR_NOTDEFINED             0x8000
+#define FOE_ERR_NOTFOUND               0x8001
+#define FOE_ERR_ACCESS                 0x8002
+#define FOE_ERR_DISKFULL               0x8003
+#define FOE_ERR_ILLEGAL                0x8004
+#define FOE_ERR_PACKETNO               0x8005
+#define FOE_ERR_EXISTS                 0x8006
+#define FOE_ERR_NOUSER                 0x8007
+#define FOE_ERR_BOOTSTRAPONLY          0x8008
+#define FOE_ERR_NOTINBOOTSTRAP         0x8009
+#define FOE_ERR_NORIGHTS               0x800A
+#define FOE_ERR_PROGERROR              0x800B
+#define FOE_ERR_CHECKSUM               0x800C
+
+#define FOE_OP_RRQ                     1
+#define FOE_OP_WRQ                     2
+#define FOE_OP_DATA                    3
+#define FOE_OP_ACK                     4
+#define FOE_OP_ERR                     5
+#define FOE_OP_BUSY                    6
+
+#define FOE_READY                      0
+#define FOE_WAIT_FOR_ACK               1
+#define FOE_WAIT_FOR_FINAL_ACK         2
+#define FOE_WAIT_FOR_DATA              3
+
+#define EOE_RESULT_SUCCESS                   0x0000
+#define EOE_RESULT_UNSPECIFIED_ERROR         0x0001
+#define EOE_RESULT_UNSUPPORTED_FRAME_TYPE    0x0002
+#define EOE_RESULT_NO_IP_SUPPORT             0x0201
+#define EOE_RESULT_NO_DHCP_SUPPORT           0x0202
+#define EOE_RESULT_NO_FILTER_SUPPORT         0x0401
+
+#define APPSTATE_IDLE                  0x00
+#define APPSTATE_INPUT                 0x01
+#define APPSTATE_OUTPUT                0x02
+
+#define PREALLOC_FACTOR       3
+#define PREALLOC_BUFFER_SIZE  (PREALLOC_FACTOR * MBXSIZE)
+
+typedef struct sm_cfg
+{
+   uint16_t cfg_sma;
+   uint16_t cfg_sml;
+   uint16_t cfg_sme;
+   uint8_t cfg_smc;
+   uint8_t cfg_smact;
+}sm_cfg_t;
+
+typedef struct esc_cfg
+{
+   void * user_arg;
+   int use_interrupt;
+   int watchdog_cnt;
+   bool skip_default_initialization;
+   void (*set_defaults_hook) (void);
+   void (*pre_state_change_hook) (uint8_t * as, uint8_t * an);
+   void (*post_state_change_hook) (uint8_t * as, uint8_t * an);
+   void (*application_hook) (void);
+   void (*safeoutput_override) (void);
+   uint32_t (*pre_object_download_hook) (uint16_t index,
+         uint8_t subindex,
+         void * data,
+         size_t size,
+         uint16_t flags);
+   uint32_t (*post_object_download_hook) (uint16_t index,
+         uint8_t subindex,
+         uint16_t flags);
+   uint32_t (*pre_object_upload_hook) (uint16_t index,
+         uint8_t subindex,
+         void * data,
+         size_t size,
+         uint16_t flags);
+   uint32_t (*post_object_upload_hook) (uint16_t index,
+         uint8_t subindex,
+         uint16_t flags);
+   void (*rxpdo_override) (void);
+   void (*txpdo_override) (void);
+   void (*esc_hw_interrupt_enable) (uint32_t mask);
+   void (*esc_hw_interrupt_disable) (uint32_t mask);
+   void (*esc_hw_eep_handler) (void);
+   uint16_t (*esc_check_dc_handler) (void);
+} esc_cfg_t;
+
+typedef struct
+{
+   uint8_t state;
+} _App;
+
+// Attention! this struct is always little-endian
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t PSA;
+   uint16_t Length;
+
+#if defined(EC_LITTLE_ENDIAN)
+   uint8_t Mode:2;
+   uint8_t Direction:2;
+   uint8_t IntECAT:1;
+   uint8_t IntPDI:1;
+   uint8_t WTE:1;
+   uint8_t R1:1;
+
+   uint8_t IntW:1;
+   uint8_t IntR:1;
+   uint8_t R2:1;
+   uint8_t MBXstat:1;
+   uint8_t BUFstat:2;
+   uint8_t R3:2;
+
+   uint8_t ECsm:1;
+   uint8_t ECrep:1;
+   uint8_t ECr4:4;
+   uint8_t EClatchEC:1;
+   uint8_t EClatchPDI:1;
+
+   uint8_t PDIsm:1;
+   uint8_t PDIrep:1;
+   uint8_t PDIr5:6;
+#endif
+
+#if defined(EC_BIG_ENDIAN)
+   uint8_t R1:1;
+   uint8_t WTE:1;
+   uint8_t IntPDI:1;
+   uint8_t IntECAT:1;
+   uint8_t Direction:2;
+   uint8_t Mode:2;
+
+   uint8_t R3:2;
+   uint8_t BUFstat:2;
+   uint8_t MBXstat:1;
+   uint8_t R2:1;
+   uint8_t IntR:1;
+   uint8_t IntW:1;
+
+   uint8_t EClatchPDI:1;
+   uint8_t EClatchEC:1;
+   uint8_t ECr4:4;
+   uint8_t ECrep:1;
+   uint8_t ECsm:1;
+
+   uint8_t PDIr5:6;
+   uint8_t PDIrep:1;
+   uint8_t PDIsm:1;
+#endif
+} _ESCsm;
+CC_PACKED_END
+
+/* Attention! this struct is always little-endian */
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t PSA;
+   uint16_t Length;
+   uint8_t Command;
+   uint8_t Status;
+   uint8_t ActESC;
+   uint8_t ActPDI;
+} _ESCsm2;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t PSA;
+   uint16_t Length;
+   uint8_t Command;
+} _ESCsmCompact;
+CC_PACKED_END
+
+typedef struct
+{
+   /* Configuration input is saved so the user variable may go out-of-scope */
+   int use_interrupt;
+   sm_cfg_t  mb[2];
+   sm_cfg_t  mbboot[2];
+   bool skip_default_initialization;
+   void (*set_defaults_hook) (void);
+   void (*pre_state_change_hook) (uint8_t * as, uint8_t * an);
+   void (*post_state_change_hook) (uint8_t * as, uint8_t * an);
+   void (*application_hook) (void);
+   void (*safeoutput_override) (void);
+   uint32_t (*pre_object_download_hook) (uint16_t index,
+         uint8_t subindex,
+         void * data,
+         size_t size,
+         uint16_t flags);
+   uint32_t (*post_object_download_hook) (uint16_t index,
+         uint8_t subindex,
+         uint16_t flags);
+   uint32_t (*pre_object_upload_hook) (uint16_t index,
+         uint8_t subindex,
+         void * data,
+         size_t size,
+         uint16_t flags);
+   uint32_t (*post_object_upload_hook) (uint16_t index,
+         uint8_t subindex,
+         uint16_t flags);
+   void (*rxpdo_override) (void);
+   void (*txpdo_override) (void);
+   void (*esc_hw_interrupt_enable) (uint32_t mask);
+   void (*esc_hw_interrupt_disable) (uint32_t mask);
+   void (*esc_hw_eep_handler) (void);
+   uint16_t (*esc_check_dc_handler) (void);
+   uint8_t MBXrun;
+   size_t activembxsize;
+   sm_cfg_t * activemb0;
+   sm_cfg_t * activemb1;
+   uint16_t ESC_SM2_sml;
+   uint16_t ESC_SM3_sml;
+   uint8_t dcsync;
+   uint16_t synccounterlimit;
+   uint16_t ALstatus;
+   uint16_t ALcontrol;
+   uint16_t ALerror;
+   uint16_t DLstatus;
+   uint16_t address;
+   uint8_t mbxcnt;
+   uint8_t mbxincnt;
+   uint8_t mbxoutpost;
+   uint8_t mbxbackup;
+   uint8_t xoe;
+   uint8_t txcue;
+   uint8_t mbxfree;
+   uint8_t segmented;
+   void *data;
+   uint16_t entries;
+   uint16_t frags;
+   uint16_t fragsleft;
+   uint16_t index;
+   uint8_t subindex;
+   uint16_t flags;
+
+   uint8_t toggle;
+
+   int sm2mappings;
+   int sm3mappings;
+
+   uint8_t SMtestresult;
+   uint32_t PrevTime;
+   _ESCsm SM[4];
+   /* Volatile since it may be read from ISR */
+   volatile int watchdogcnt;
+   volatile uint32_t Time;
+   volatile uint16_t ALevent;
+   volatile int8_t synccounter;
+   volatile _App App;
+   uint8_t mbxdata[PREALLOC_BUFFER_SIZE];
+} _ESCvar;
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t length;
+   uint16_t address;
+
+#if defined(EC_LITTLE_ENDIAN)
+   uint8_t channel:6;
+   uint8_t priority:2;
+
+   uint8_t mbxtype:4;
+   uint8_t mbxcnt:4;
+#endif
+
+#if defined(EC_BIG_ENDIAN)
+   uint8_t priority:2;
+   uint8_t channel:6;
+
+   uint8_t mbxcnt:4;
+   uint8_t mbxtype:4;
+#endif
+} _MBXh;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh header;
+   uint8_t b[0];
+} _MBX;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t numberservice;
+} _COEh;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+#if defined(EC_LITTLE_ENDIAN)
+   uint8_t opcode:7;
+   uint8_t incomplete:1;
+#endif
+
+#if defined(EC_BIG_ENDIAN)
+   uint8_t incomplete:1;
+   uint8_t opcode:7;
+#endif
+
+   uint8_t reserved;
+   uint16_t fragmentsleft;
+} _INFOh;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   uint16_t type;
+   uint16_t detail;
+} _MBXerr;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   _COEh coeheader;
+   uint8_t command;
+   uint16_t index;
+   uint8_t subindex;
+   uint32_t size;
+} _COEsdo;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   _COEh coeheader;
+   _INFOh infoheader;
+   uint16_t index;
+   uint16_t datatype;
+   uint8_t maxsub;
+   uint8_t objectcode;
+   char name;
+} _COEobjdesc;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   _COEh coeheader;
+   _INFOh infoheader;
+   uint16_t index;
+   uint8_t subindex;
+   uint8_t valueinfo;
+   uint16_t datatype;
+   uint16_t bitlength;
+   uint16_t access;
+   char name;
+} _COEentdesc;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint8_t opcode;
+   uint8_t reserved;
+   union
+   {
+      uint32_t password;
+      uint32_t packetnumber;
+      uint32_t errorcode;
+   };
+} _FOEh;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   _FOEh foeheader;
+   union
+   {
+      char filename[0];
+      uint8_t data[0];
+      char errortext[0];
+   };
+} _FOE;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t frameinfo1;
+   union
+   {
+      uint16_t frameinfo2;
+      uint16_t result;
+   };
+} _EOEh;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   _MBXh mbxheader;
+   _EOEh eoeheader;
+   uint8_t data[0];
+} _EOE;
+CC_PACKED_END
+
+/* state definition in mailbox
+ * 0 : idle
+ * 1 : claimed for inbox
+ * 2 : claimed for outbox
+ * 3 : request post outbox
+ * 4 : outbox posted not send
+ * 5 : backup outbox
+ * 6 : mailbox needs to be transmitted again
+ */
+typedef struct
+{
+   uint8_t state;
+} _MBXcontrol;
+
+/* Stack reference to application configuration of the ESC */
+#define ESC_MBXSIZE         (ESCvar.activembxsize)
+#define ESC_MBX0_sma        (ESCvar.activemb0->cfg_sma)
+#define ESC_MBX0_sml        (ESCvar.activemb0->cfg_sml)
+#define ESC_MBX0_sme        (ESCvar.activemb0->cfg_sme)
+#define ESC_MBX0_smc        (ESCvar.activemb0->cfg_smc)
+#define ESC_MBX1_sma        (ESCvar.activemb1->cfg_sma)
+#define ESC_MBX1_sml        (ESCvar.activemb1->cfg_sml)
+#define ESC_MBX1_sme        (ESCvar.activemb1->cfg_sme)
+#define ESC_MBX1_smc        (ESCvar.activemb1->cfg_smc)
+#define ESC_MBXBUFFERS      (MBXBUFFERS)
+#define ESC_SM2_sma         (SM2_sma)
+#define ESC_SM2_smc         (SM2_smc)
+#define ESC_SM2_act         (SM2_act)
+#define ESC_SM3_sma         (SM3_sma)
+#define ESC_SM3_smc         (SM3_smc)
+#define ESC_SM3_act         (SM3_act)
+
+#define ESC_MBXHSIZE        sizeof(_MBXh)
+#define ESC_MBXDSIZE        (ESC_MBXSIZE - ESC_MBXHSIZE)
+#define ESC_FOEHSIZE        sizeof(_FOEh)
+#define ESC_FOE_DATA_SIZE   (ESC_MBXSIZE - (ESC_MBXHSIZE +ESC_FOEHSIZE))
+#define ESC_EOEHSIZE        sizeof(_EOEh)
+#define ESC_EOE_DATA_SIZE   (ESC_MBXSIZE - (ESC_MBXHSIZE +ESC_EOEHSIZE))
+
+void ESC_config (esc_cfg_t * cfg);
+void ESC_ALerror (uint16_t errornumber);
+void ESC_ALeventwrite (uint32_t event);
+uint32_t ESC_ALeventread (void);
+void ESC_ALeventmaskwrite (uint32_t mask);
+uint32_t ESC_ALeventmaskread (void);
+void ESC_ALstatus (uint8_t status);
+void ESC_ALstatusgotoerror (uint8_t status, uint16_t errornumber);
+void ESC_SMstatus (uint8_t n);
+uint8_t ESC_WDstatus (void);
+uint8_t ESC_claimbuffer (void);
+uint8_t ESC_startmbx (uint8_t state);
+void ESC_stopmbx (void);
+void MBX_error (uint16_t error);
+uint8_t ESC_mbxprocess (void);
+void ESC_xoeprocess (void);
+uint8_t ESC_startinput (uint8_t state);
+void ESC_stopinput (void);
+uint8_t ESC_startoutput (uint8_t state);
+void ESC_stopoutput (void);
+void ESC_state (void);
+void ESC_sm_act_event (void);
+
+/* From hardware file */
+void ESC_read (uint16_t address, void *buf, uint16_t len);
+void ESC_write (uint16_t address, void *buf, uint16_t len);
+void ESC_init (const esc_cfg_t * cfg);
+void ESC_reset (void);
+
+/* From application */
+extern void APP_safeoutput ();
+extern _ESCvar ESCvar;
+extern _MBXcontrol MBXcontrol[];
+extern uint8_t MBX[];
+extern _SMmap SMmap2[];
+extern _SMmap SMmap3[];
+
+/* ATOMIC operations are used when running interrupt driven */
+#ifndef CC_ATOMIC_SET
+#define CC_ATOMIC_SET(var,val)   (var = val)
+#endif
+
+#ifndef CC_ATOMIC_GET
+#define CC_ATOMIC_GET(var)       (var)
+#endif
+
+#ifndef CC_ATOMIC_ADD
+#define CC_ATOMIC_ADD(var,val)   (var += val)
+#endif
+
+#ifndef CC_ATOMIC_SUB
+#define CC_ATOMIC_SUB(var,val)   (var -= val)
+#endif
+
+#ifndef CC_ATOMIC_AND
+#define CC_ATOMIC_AND(var,val)   (var &= val)
+#endif
+
+#ifndef CC_ATOMIC_OR
+#define CC_ATOMIC_OR(var,val)    (var |= val)
+#endif
+
+
+#endif

Cards/EaserCAT-2000/Firmware/lib/soes/esc_coe.h

@@ -0,0 +1,135 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+ /** \file
+ * \brief
+ * Headerfile for esc_coe.c
+ */
+
+#ifndef __esc_coe__
+#define __esc_coe__
+
+#include <cc.h>
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t subindex;
+   uint16_t datatype;
+   uint16_t bitlength;
+   uint16_t flags;
+   const char *name;
+   uint32_t value;
+   void *data;
+} _objd;
+CC_PACKED_END
+
+CC_PACKED_BEGIN
+typedef struct CC_PACKED
+{
+   uint16_t index;
+   uint16_t objtype;
+   uint8_t maxsub;
+   uint8_t pad1;
+   const char *name;
+   const _objd *objdesc;
+} _objectlist;
+CC_PACKED_END
+
+typedef struct
+{
+   const _objd * obj;
+   uint16_t offset;
+} _SMmap;
+
+#define OBJH_READ               0
+#define OBJH_WRITE              1
+
+#define OTYPE_DOMAIN            0x0002
+#define OTYPE_DEFTYPE           0x0005
+#define OTYPE_DEFSTRUCT         0x0006
+#define OTYPE_VAR               0x0007
+#define OTYPE_ARRAY             0x0008
+#define OTYPE_RECORD            0x0009
+
+#define DTYPE_BOOLEAN           0x0001
+#define DTYPE_INTEGER8          0x0002
+#define DTYPE_INTEGER16         0x0003
+#define DTYPE_INTEGER32         0x0004
+#define DTYPE_UNSIGNED8         0x0005
+#define DTYPE_UNSIGNED16        0x0006
+#define DTYPE_UNSIGNED32        0x0007
+#define DTYPE_REAL32            0x0008
+#define DTYPE_VISIBLE_STRING    0x0009
+#define DTYPE_OCTET_STRING      0x000A
+#define DTYPE_UNICODE_STRING    0x000B
+#define DTYPE_INTEGER24         0x0010
+#define DTYPE_UNSIGNED24        0x0016
+#define DTYPE_INTEGER64         0x0015
+#define DTYPE_UNSIGNED64        0x001B
+#define DTYPE_REAL64            0x0011
+#define DTYPE_PDO_MAPPING       0x0021
+#define DTYPE_IDENTITY          0x0023
+#define DTYPE_BITARR8           0x002D
+#define DTYPE_BITARR16          0x002E
+#define DTYPE_BITARR32          0x002F
+#define DTYPE_BIT1              0x0030
+#define DTYPE_BIT2              0x0031
+#define DTYPE_BIT3              0x0032
+#define DTYPE_BIT4              0x0033
+#define DTYPE_BIT5              0x0034
+#define DTYPE_BIT6              0x0035
+#define DTYPE_BIT7              0x0036
+#define DTYPE_BIT8              0x0037
+#define DTYPE_ARRAY_OF_INT      0x0260
+#define DTYPE_ARRAY_OF_SINT     0x0261
+#define DTYPE_ARRAY_OF_DINT     0x0262
+#define DTYPE_ARRAY_OF_UDINT    0x0263
+
+#define ATYPE_Rpre              0x01
+#define ATYPE_Rsafe             0x02
+#define ATYPE_Rop               0x04
+#define ATYPE_Wpre              0x08
+#define ATYPE_Wsafe             0x10
+#define ATYPE_Wop               0x20
+#define ATYPE_RXPDO             0x40
+#define ATYPE_TXPDO             0x80
+
+#define ATYPE_RO                (ATYPE_Rpre | ATYPE_Rsafe | ATYPE_Rop)
+#define ATYPE_WO                (ATYPE_Wpre | ATYPE_Wsafe | ATYPE_Wop)
+#define ATYPE_RW                (ATYPE_RO | ATYPE_WO)
+#define ATYPE_RWpre             (ATYPE_Wpre | ATYPE_RO)
+
+#define TX_PDO_OBJIDX           0x1c13
+#define RX_PDO_OBJIDX           0x1c12
+
+#define COMPLETE_ACCESS_FLAG    (1 << 15)
+
+void ESC_coeprocess (void);
+int16_t SDO_findsubindex (int16_t nidx, uint8_t subindex);
+int32_t SDO_findobject (uint16_t index);
+uint16_t sizeOfPDO (uint16_t index, int * nmappings, _SMmap * sm, int max_mappings);
+void SDO_abort (uint16_t index, uint8_t subindex, uint32_t abortcode);
+void COE_initDefaultValues (void);
+
+void COE_pdoPack (uint8_t * buffer, int nmappings, _SMmap * sm);
+void COE_pdoUnpack (uint8_t * buffer, int nmappings, _SMmap * sm);
+uint8_t COE_maxSub (uint16_t index);
+
+extern uint32_t ESC_download_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags);
+extern uint32_t ESC_download_pre_objecthandler (uint16_t index,
+      uint8_t subindex,
+      void * data,
+      size_t size,
+      uint16_t flags);
+extern uint32_t ESC_upload_pre_objecthandler (uint16_t index,
+      uint8_t subindex,
+      void * data,
+      size_t size,
+      uint16_t flags);
+extern uint32_t ESC_upload_post_objecthandler (uint16_t index, uint8_t subindex, uint16_t flags);
+extern const _objectlist SDOobjects[];
+
+#endif

Cards/EaserCAT-2000/Firmware/lib/soes/esc_eep.c

@@ -0,0 +1,80 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+ /** \file
+ * \brief
+ * ESI EEPROM emulator module.
+ */
+
+#include "cc.h"
+#include "esc.h"
+#include "esc_eep.h"
+
+#include <string.h>
+
+static uint8_t eep_buf[8];
+
+/** EPP periodic task of ESC side EEPROM emulation.
+ *
+ */
+void EEP_process (void)
+{
+   eep_stat_t stat;
+
+   /* check for eeprom event */
+   if ((ESCvar.ALevent & ESCREG_ALEVENT_EEP) == 0) {
+     return;
+   }
+
+   while (1) {
+      /* read eeprom status */
+      ESC_read (ESCREG_EECONTSTAT, &stat, sizeof (eep_stat_t));
+      stat.contstat.reg = etohs(stat.contstat.reg);
+      stat.addr = etohl(stat.addr);
+
+      /* check busy flag, exit if job finished */
+      if (!stat.contstat.bits.busy) {
+        return;
+      }
+
+      /* clear error bits */
+      stat.contstat.bits.csumErr = 0;
+      stat.contstat.bits.eeLoading = 0;
+      stat.contstat.bits.ackErr = 0;
+      stat.contstat.bits.wrErr = 0;
+
+      /* process commands */
+      switch (stat.contstat.bits.cmdReg) {
+         case EEP_CMD_IDLE:
+            break;
+
+         case EEP_CMD_READ:
+         case EEP_CMD_RELOAD:
+            /* handle read request */
+            if (EEP_read (stat.addr * sizeof(uint16_t), eep_buf, EEP_READ_SIZE) != 0) {
+               stat.contstat.bits.ackErr = 1;
+            } else {
+               ESC_write (ESCREG_EEDATA, eep_buf, EEP_READ_SIZE);
+            }
+            break;
+
+         case EEP_CMD_WRITE:
+            /* handle write request */
+            ESC_read (ESCREG_EEDATA, eep_buf, EEP_WRITE_SIZE);
+            if (EEP_write (stat.addr * sizeof(uint16_t), eep_buf, EEP_WRITE_SIZE) != 0) {
+               stat.contstat.bits.ackErr = 1;
+            }
+            break;
+
+         default:
+            stat.contstat.bits.ackErr = 1;
+      }
+
+      /* acknowledge command */
+      stat.contstat.reg = htoes(stat.contstat.reg);
+      ESC_write (ESCREG_EECONTSTAT, &stat.contstat.reg, sizeof(uint16_t));
+   }
+}
+

Cards/EaserCAT-2000/Firmware/lib/soes/esc_eep.h

@@ -0,0 +1,77 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+/** \file
+ * \brief
+ * Headerfile for esc_eep.c
+ */
+
+#ifndef __esc_eep__
+#define __esc_eep__
+
+#include <cc.h>
+#include "esc.h"
+
+/* EEPROM commands */
+#define EEP_CMD_IDLE        0x0
+#define EEP_CMD_READ        0x1
+#define EEP_CMD_WRITE       0x2
+#define EEP_CMD_RELOAD      0x3
+
+/* read/write size */
+#define EEP_READ_SIZE       8
+#define EEP_WRITE_SIZE      2
+
+/* CONSTAT register content */
+typedef struct CC_PACKED
+{
+   union {
+     uint16_t reg;
+     struct {
+        uint8_t wrEnable:1;
+        uint8_t reserved:4;
+        uint8_t eeEmulated:1;
+        uint8_t eightByteRead:1;
+        uint8_t twoByteAddr:1;
+
+        uint8_t cmdReg:3;
+        uint8_t csumErr:1;
+        uint8_t eeLoading:1;
+        uint8_t ackErr:1;
+        uint8_t wrErr:1;
+        uint8_t busy:1;
+      } bits;
+   } contstat;
+
+   uint32_t addr;
+} eep_stat_t;
+
+/**
+ * ECAT EEPROM configuration area data structure
+ */
+typedef union eep_config
+{
+  struct
+  {
+    uint16_t pdi_control;
+    uint16_t pdi_configuration;
+    uint16_t sync_impulse_len;
+    uint16_t pdi_configuration2;
+    uint16_t configured_station_alias;
+    uint8_t  reserved[4];
+    uint16_t checksum;
+  };
+  uint32_t dword[4]; /**< Four 32 bit double word equivalent to 8 16 bit configuration area word. */
+}eep_config_t;
+
+/* periodic task */
+void EEP_process (void);
+
+/* From hardware file */
+void EEP_init (void);
+int8_t EEP_read (uint32_t addr, uint8_t *data, uint16_t size);
+int8_t EEP_write (uint32_t addr, uint8_t *data, uint16_t size);
+
+#endif

Cards/EaserCAT-2000/Firmware/lib/soes/esc_eoe.h

@@ -0,0 +1,68 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+/** \file
+ * \brief
+ * Headerfile for esc_eoe.c
+ */
+
+#ifndef __esc_eoe__
+#define __esc_eoe__
+
+#include <cc.h>
+
+typedef struct eoe_pbuf
+{
+   /** Pointer to frame buffer type used by a TCP/IP stack. (Not mandatory) */
+   void * pbuf;
+   /** Pointer to frame buffer to send or read from */
+   uint8_t * payload;
+   /** Length of data in frame buffer */
+   size_t len;
+} eoe_pbuf_t;
+
+typedef struct eoe_cfg
+{
+   /** Callback function to get a frame buffer for storage of received frame */
+   void (*get_buffer) (eoe_pbuf_t * ebuf);
+   /** Callback function to free a frame buffer */
+   void (*free_buffer) (eoe_pbuf_t * ebuf);
+   /** Callback function to read local settings and update EtherCAT variables
+    *  to be delivered to the EtherCAT Master
+    */
+   int  (*load_eth_settings) (void);
+   /** Callback function to read settings provided by the EtherCAT master
+    * and store to local settings.
+    */
+   int  (*store_ethernet_settings) (void);
+   /** Callback to frame receive function in TCP(IP stack,
+    *  caller should free the buffer
+    * */
+   void (*handle_recv_buffer) (uint8_t port, eoe_pbuf_t * ebuf);
+   /** Callback to fetch a buffer to send */
+   int (*fetch_send_buffer) (uint8_t port, eoe_pbuf_t * ebuf);
+   /** Callback to notify the application fragment sent */
+   void (*fragment_sent_event) (void);
+} eoe_cfg_t;
+
+int EOE_ecat_get_mac (uint8_t port, uint8_t mac[]);
+int EOE_ecat_get_ip (uint8_t port, uint32_t * ip);
+int EOE_ecat_get_subnet (uint8_t port, uint32_t * subnet);
+int EOE_ecat_get_gateway (uint8_t port, uint32_t * default_gateway);
+int EOE_ecat_get_dns_ip (uint8_t port, uint32_t * dns_ip);
+int EOE_ecat_get_dns_name (uint8_t port, char * dns_name);
+int EOE_ecat_set_mac (uint8_t port, uint8_t mac[]);
+int EOE_ecat_set_ip (uint8_t port, uint32_t ip);
+int EOE_ecat_set_subnet (uint8_t port, uint32_t subnet);
+int EOE_ecat_set_gateway (uint8_t port, uint32_t default_gateway);
+int EOE_ecat_set_dns_ip (uint8_t port, uint32_t dns_ip);
+int EOE_ecat_set_dns_name (uint8_t port, char * dns_name);
+
+void EOE_config (eoe_cfg_t * cfg);
+void EOE_init (void);
+void ESC_eoeprocess (void);
+void ESC_eoeprocess_tx (void);
+
+#endif

Cards/EaserCAT-2000/Firmware/lib/soes/esc_foe.c

@@ -0,0 +1,627 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+ /** \file
+ * \brief
+ * File over EtherCAT (FoE) module.
+ */
+
+
+#include <cc.h>
+#include "esc.h"
+#include "esc_foe.h"
+#include <string.h>
+
+ /** \file
+ * \brief
+ * File over EtherCAT (FoE) module.
+ *
+ * FOE read / write and FOE service functions
+ */
+
+//define if FOE_read should be supported
+//#define FOE_READ_SUPPORTED
+
+/** Variable holding current filename read at FOE Open.
+ */
+static char foe_file_name[FOE_FN_MAX + 1];
+
+
+/** Main FoE configuration pointer data array. Structure is allocated and filled
+ * by the application defining what preferences it requires.
+ */
+static foe_cfg_t * foe_cfg;
+/** Pointer to current file configuration item used by FoE.
+ */
+static foe_file_cfg_t * foe_file;
+/** Main FoE status data array. Structure gets filled with current status
+ * variables during FoE usage.
+ */
+static _FOEvar FOEvar;
+
+/** Validate a write or read request by checking filename and password.
+ *
+ * @param[in] name      = Filename
+ * @param[in] num_chars = Length of filename
+ * @param[in] pass      = Numeric variable of password
+ * @param[in] op        = Request op-code
+ * @return 0= if we succeed, FOE_ERR_NOTFOUND something wrong with filename or
+ * password
+ */
+static int FOE_fopen (char *name, uint8_t num_chars, uint32_t pass, uint8_t op)
+{
+   uint32_t i;
+
+   /* Unpack the file name into characters we can look at. */
+   if (num_chars > FOE_FN_MAX)
+   {
+      num_chars = FOE_FN_MAX;
+   }
+
+   for (i = 0; i < num_chars; i++)
+   {
+      foe_file_name[i] = name[i];
+   }
+   foe_file_name[i] = '\0';
+
+   /* Figure out what file they're talking about. */
+   for (i = 0; i < foe_cfg->n_files; i++)
+   {
+      if (0 == strncmp (foe_file_name, foe_cfg->files[i].name, num_chars))
+      {
+         if (pass != foe_cfg->files[i].filepass)
+         {
+            return FOE_ERR_NORIGHTS;
+         }
+
+         if (op == FOE_OP_WRQ &&
+             (foe_cfg->files[i].write_only_in_boot) &&
+             (ESCvar.ALstatus != ESCboot))
+         {
+            return FOE_ERR_NOTINBOOTSTRAP;
+         }
+
+         foe_file = &foe_cfg->files[i];
+         foe_file->address_offset = 0;
+         foe_file->total_size = 0;
+         switch (op)
+         {
+            case FOE_OP_RRQ:
+            {
+               FOEvar.fposition = 0;
+               FOEvar.fend = foe_cfg->files[i].max_data;
+               return 0;
+            }
+            case FOE_OP_WRQ:
+            {
+               FOEvar.fposition = 0;
+               FOEvar.fend = foe_cfg->files[i].max_data;
+               return 0;
+            }
+         }
+      }
+   }
+
+   return FOE_ERR_NOTFOUND;
+}
+
+#ifdef FOE_READ_SUPPORTED
+/** Function writing local data to mailbox buffer to be sent as next FoE frame.
+ * It will try to fill the Mailbox buffer available if there is enough data
+ * left to read.
+ *
+ * @param[in] data      = pointer to buffer
+ * @param[in] maxlength = max length of data possible to read, controlled by
+ * Mailbox - FoE and Mailbox frame headers.
+
+ * @return Number of copied bytes.
+ */
+static uint16_t FOE_fread (uint8_t * data, uint16_t maxlength)
+{
+   uint16_t ncopied = 0;
+
+   while (maxlength && (FOEvar.fend - FOEvar.fposition))
+   {
+      maxlength--;
+      *(data++) = foe_cfg->fbuffer[FOEvar.fposition++];
+      ncopied++;
+   }
+
+   return ncopied;
+}
+#endif
+
+/** Function reading mailbox buffer to local buffer to be handled by
+ * application write hook. Ex. flash routine used by software update.
+ * It will consume the buffer and call the write hook every time the configured
+ * flush buffer limit is reached.
+ *
+ *
+ * @param[in] data   = Pointer to buffer
+ * @param[in] length = Length of data to read
+
+ * @return Number of copied bytes.
+ */
+static uint16_t FOE_fwrite (uint8_t *data, uint16_t length)
+{
+    uint16_t ncopied = 0;
+    uint32_t failed = 0;
+
+    DPRINT("FOE_fwrite\n");
+    FOEvar.fprevposition = FOEvar.fposition;
+    while (length && (FOEvar.fend - FOEvar.fposition) && !failed)
+    {
+       length--;
+       foe_cfg->fbuffer[FOEvar.fbufposition++] = *(data++);
+       if(FOEvar.fbufposition >= foe_cfg->buffer_size)
+       {
+          failed = foe_file->write_function (foe_file, foe_cfg->fbuffer, FOEvar.fbufposition);
+          FOEvar.fbufposition = 0;
+          foe_file->address_offset += foe_cfg->buffer_size;
+       }
+       FOEvar.fposition++;
+       ncopied++;
+    }
+
+    foe_file->total_size += ncopied;
+
+    DPRINT("FOE_fwrite END with : %d\n",ncopied);
+    return ncopied;
+}
+
+
+/** Function handling the final FOE_fwrite when we close up regardless
+ * if we have filled the buffers or not.
+ *
+ * @return Number of copied bytes on success, 0= if failed.
+ */
+static uint32_t FOE_fclose (void)
+{
+   uint32_t failed = 0;
+
+   DPRINT("FOE_fclose\n");
+   
+   failed = foe_file->write_function (foe_file, foe_cfg->fbuffer, FOEvar.fbufposition);
+   foe_file->address_offset += FOEvar.fbufposition;
+   FOEvar.fbufposition = 0;
+
+   return failed;
+}
+
+/** Initialize by clearing all current status variables.
+ *
+ */
+void FOE_init ()
+{
+   DPRINT("FOE_init\n");
+   FOEvar.foepacket = 0;
+   FOEvar.foestate = FOE_READY;
+   FOEvar.fposition = 0;
+   FOEvar.fprevposition = 0;
+   FOEvar.fbufposition = 0;
+}
+
+/** Function for sending an FOE abort frame.
+ *
+ * @param[in] code   = abort code
+ */
+static void FOE_abort (uint32_t code)
+{
+   _FOE *foembx;
+   uint8_t mbxhandle;
+
+   if (code)
+   {
+      /* Send back an error packet. */
+      mbxhandle = ESC_claimbuffer ();
+      if (mbxhandle)
+      {
+         foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
+         foembx->mbxheader.length = htoes (ESC_FOEHSIZE);   /* Don't bother with error text for now. */
+         foembx->mbxheader.mbxtype = MBXFOE;
+         foembx->foeheader.opcode = FOE_OP_ERR;
+         foembx->foeheader.errorcode = htoel (code);
+         MBXcontrol[mbxhandle].state = MBXstate_outreq;
+      }
+      /* Nothing we can do if we can't get an outbound mailbox. */
+   }
+   DPRINT("FOE_abort: 0x%X\n", code);
+   FOE_init ();
+}
+
+#ifdef FOE_READ_SUPPORTED
+/** Sends an FoE data frame, returning the number of data bytes
+ * written or an error number.
+ * Error numbers will be greater than FOE_DATA_SIZE.
+
+ * @param[in] data   = pointer to buffer
+ * @param[in] length = length of data to read
+
+ * @return Number of data bytes written or an error number. Error numbers
+ * will be greater than FOE_DATA_SIZE.
+ */
+static int FOE_send_data_packet ()
+{
+   _FOE *foembx;
+   uint16_t data_len;
+   uint8_t mbxhandle;
+
+   mbxhandle = ESC_claimbuffer ();
+   if (mbxhandle)
+   {
+      foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
+      data_len = FOE_fread (foembx->data, ESC_FOE_DATA_SIZE);
+      foembx->foeheader.opcode = FOE_OP_DATA;
+      foembx->foeheader.packetnumber = htoel (FOEvar.foepacket);
+      FOEvar.foepacket++;
+      foembx->mbxheader.length = htoes (data_len + ESC_FOEHSIZE);
+      foembx->mbxheader.mbxtype = MBXFOE;
+      /* Mark the outbound mailbox as filled. */
+      MBXcontrol[mbxhandle].state = MBXstate_outreq;
+      return data_len;
+   }
+   else
+   {
+      return FOE_ERR_PROGERROR;
+   }
+}
+#endif
+
+/** Sends an FoE ack data frame.
+
+ * @return 0= or error number.
+ */
+static int FOE_send_ack ()
+{
+   _FOE *foembx;
+   uint8_t mbxhandle;
+
+   mbxhandle = ESC_claimbuffer ();
+   if (mbxhandle)
+   {
+      DPRINT("FOE_send_ack\n");
+      foembx = (_FOE *) &MBX[mbxhandle * ESC_MBXSIZE];
+      foembx->mbxheader.length = htoes (ESC_FOEHSIZE);
+      foembx->mbxheader.mbxtype = MBXFOE;
+      foembx->foeheader.opcode = FOE_OP_ACK;
+      foembx->foeheader.packetnumber = htoel (FOEvar.foepacket);
+      FOEvar.foepacket++;
+      MBXcontrol[mbxhandle].state = MBXstate_outreq;
+      return 0;
+   }
+   else
+   {
+      DPRINT("ERROR:FOE_send_ack\n");
+      return FOE_ERR_PROGERROR;
+   }
+}
+
+/* Handlers for various FoE states. */
+
+#ifdef FOE_READ_SUPPORTED
+/** FoE read request handler. Starts with Initialize, Open and Sending one frame.
+ * When first frame have been sent we will send data from Ack.
+ * On error we will send FOE Abort.
+ *
+ */
+static void FOE_read ()
+{
+   _FOE *foembx;
+   uint32_t data_len;
+   uint32_t password;
+   int res;
+
+   if (FOEvar.foestate != FOE_READY)
+   {
+      FOE_abort (FOE_ERR_ILLEGAL);
+      return;
+   }
+
+   FOE_init ();
+   foembx = (_FOE *) &MBX[0];
+   /* Get the length of the file name in octets. */
+   data_len = etohs (foembx->mbxheader.length) - ESC_FOEHSIZE;
+   password = etohl (foembx->foeheader.password);
+
+   res = FOE_fopen (foembx->filename, data_len, password, FOE_OP_RRQ);
+   if (res == 0)
+   {
+      FOEvar.foepacket = 1;
+      /*
+       * Attempt to send the packet
+       */
+      res = FOE_send_data_packet ();
+      if (res <= (int)ESC_FOE_DATA_SIZE)
+      {
+         FOEvar.foestate = FOE_WAIT_FOR_ACK;
+      }
+      else
+      {
+         FOE_abort (res);
+      }
+   }
+   else
+   {
+      FOE_abort (res);
+   }
+}
+
+/** FoE data ack handler. Will continue sending next frame until finished.
+ * On error we will send FOE Abort.
+ */
+static void FOE_ack ()
+{
+   int res;
+
+   /* Make sure we're able to take this. */
+   if (FOEvar.foestate == FOE_WAIT_FOR_FINAL_ACK)
+   {
+      /* Move us back to ready state. */
+      FOE_init ();
+      return;
+   }
+   else if (FOEvar.foestate != FOE_WAIT_FOR_ACK)
+   {
+      FOE_abort (FOE_ERR_ILLEGAL);
+      return;
+   }
+   res = FOE_send_data_packet ();
+   if (res < (int)ESC_FOE_DATA_SIZE)
+   {
+      FOEvar.foestate = FOE_WAIT_FOR_FINAL_ACK;
+   }
+   else if (res >= FOE_ERR_NOTDEFINED)
+   {
+      FOE_abort (FOE_ERR_PROGERROR);
+   }
+}
+#endif
+
+/** FoE write request handler. Starts with Initialize, Open and Ack that we can/will
+ * receive data. On error we will send FOE Abort.
+ *
+ */
+static void FOE_write ()
+{
+   _FOE *foembx;
+   uint32_t data_len;
+   uint32_t password;
+   int res;
+
+   if (FOEvar.foestate != FOE_READY)
+   {
+      FOE_abort (FOE_ERR_ILLEGAL);
+      return;
+   }
+
+   FOE_init ();
+   foembx = (_FOE *) &MBX[0];
+   data_len = etohs (foembx->mbxheader.length) - ESC_FOEHSIZE;
+   password = etohl (foembx->foeheader.password);
+
+   /* Get an address we can write the file to, if possible. */
+   res = FOE_fopen (foembx->filename, data_len, password, FOE_OP_WRQ);
+   DPRINT("%s %sOK, file \"%s\"\n", __func__, (res == 0) ? "" : "N", foe_file_name);
+   if (res == 0)
+   {
+      res = FOE_send_ack ();
+      if (res)
+      {
+         FOE_abort (res);
+      }
+      else
+      {
+         FOEvar.foestate = FOE_WAIT_FOR_DATA;
+      }
+   }
+   else
+   {
+      FOE_abort (res);
+   }
+}
+/** FoE data request handler. Validates and reads data until we're finished. Every
+ * read frame followed by an Ack frame. On error we will send FOE Abort.
+ *
+ */
+static void FOE_data ()
+{
+   _FOE *foembx;
+   uint32_t packet;
+   uint16_t data_len, ncopied;
+   int res;
+
+   if(FOEvar.foestate != FOE_WAIT_FOR_DATA)
+   {
+      FOE_abort(FOE_ERR_ILLEGAL);
+      return;
+   }
+
+   foembx = (_FOE*)&MBX[0];
+   data_len = etohs(foembx->mbxheader.length) - ESC_FOEHSIZE;
+   packet = etohl(foembx->foeheader.packetnumber);
+
+   if (packet != FOEvar.foepacket)
+   {
+      DPRINT("FOE_data packet error, packet: %d, foeheader.packet: %d\n",packet,FOEvar.foepacket);
+      FOE_abort (FOE_ERR_PACKETNO);
+   }
+   else if (data_len == 0)
+   {
+      DPRINT("FOE_data completed\n");
+      FOE_fclose ();
+      res = FOE_send_ack ();
+      FOE_init ();
+   }
+   else if (FOEvar.fposition + data_len > FOEvar.fend)
+   {
+      DPRINT("FOE_data disk full\n");
+      FOE_abort (FOE_ERR_DISKFULL);
+   }
+   else
+   {
+      ncopied = FOE_fwrite (foembx->data, data_len);
+      if (!ncopied)
+      {
+         DPRINT("FOE_data no copied\n");
+         FOE_abort (FOE_ERR_PROGERROR);
+      }
+      else if (data_len == ESC_FOE_DATA_SIZE)
+      {
+         DPRINT("FOE_data data_len == FOE_DATA_SIZE\n");
+         if (ncopied != data_len)
+         {
+            DPRINT("FOE_data only %d of %d copied\n",ncopied, data_len);
+            FOE_abort (FOE_ERR_PROGERROR);
+         }
+         res = FOE_send_ack ();
+         if (res)
+         {
+            FOE_abort (res);
+         }
+      }
+      else
+      {
+         if ((ncopied != data_len) || FOE_fclose ())
+         {
+            DPRINT("FOE_fclose failed to write extra buffer\n");
+            FOE_abort (FOE_ERR_PROGERROR);
+         }
+         else
+         {
+            DPRINT("FOE_data completed\n");
+            res = FOE_send_ack ();
+            FOE_init ();
+         }
+      }
+   }
+}
+
+#ifdef FOE_READ_SUPPORTED
+/** FoE read request busy handler. Send an Ack of last frame again. On error
+ * we will send FOE Abort.
+ *
+ */
+static void FOE_busy ()
+{
+   /* Only valid if we're servicing a read request. */
+   if (FOEvar.foestate != FOE_WAIT_FOR_ACK)
+   {
+      FOE_abort (FOE_ERR_ILLEGAL);
+   }
+   else
+   {
+      /* Send the last part again. */
+      FOEvar.fposition = FOEvar.fprevposition;
+      FOEvar.foepacket--;
+      FOE_ack ();
+   }
+}
+#endif
+
+/** FoE error requesthandler. Send an FOE Abort.
+ *
+ */
+static void FOE_error ()
+{
+   /* Master panic! abort the transfer. */
+   FOE_abort (0);
+}
+
+/** Function copying the application configuration variable
+ * to the FoE module local pointer variable.
+ *
+ * @param[in] cfg       = Pointer to by the Application static declared
+ * configuration variable holding application specific details.
+ */
+void FOE_config (foe_cfg_t * cfg)
+{
+   foe_cfg = cfg;
+}
+
+/** Main FoE function checking the status on current mailbox buffers carrying
+ * data, distributing the mailboxes to appropriate FOE functions depending
+ * on requested opcode.
+ * On Error an FoE Error or FoE Abort will be sent.
+ */
+void ESC_foeprocess (void)
+{
+   _MBXh *mbh;
+   _FOE *foembx;
+
+   if (ESCvar.MBXrun == 0)
+   {
+      return;
+   }
+   if (!ESCvar.xoe && (MBXcontrol[0].state == MBXstate_inclaim))
+   {
+      mbh = (_MBXh *) &MBX[0];
+      if (mbh->mbxtype == MBXFOE)
+      {
+         ESCvar.xoe = MBXFOE;
+      }
+   }
+   if (ESCvar.xoe == MBXFOE)
+   {
+      foembx = (_FOE *) &MBX[0];
+      /* Verify the size of the file data. */
+      if (etohs (foembx->mbxheader.length) < ESC_FOEHSIZE)
+      {
+         FOE_abort (MBXERR_SIZETOOSHORT);
+      }
+      else
+      {
+         switch (foembx->foeheader.opcode)
+         {
+            case FOE_OP_WRQ:
+            {
+               DPRINT("FOE_OP_WRQ\n");
+               FOE_write ();
+               break;
+            }
+            case FOE_OP_DATA:
+            {
+               DPRINT("FOE_OP_DATA\n");
+               FOE_data ();
+               break;
+            }
+#ifdef FOE_READ_SUPPORTED
+            case FOE_OP_RRQ:
+            {
+               DPRINT("FOE_OP_RRQ\n");
+               FOE_read ();
+               break;
+            }
+            case FOE_OP_ACK:
+            {
+               DPRINT("FOE_OP_ACK\n");
+               FOE_ack ();
+               break;
+            }
+
+            case FOE_OP_BUSY:
+            {
+               DPRINT("FOE_OP_BUSY\n");
+               FOE_busy ();
+               break;
+            }
+#endif
+            case FOE_OP_ERR:
+            {
+               DPRINT("FOE_OP_ERR\n");
+               FOE_error ();
+               break;
+            }
+            default:
+            {
+               DPRINT("FOE_ERR_NOTDEFINED\n");
+               FOE_abort (FOE_ERR_NOTDEFINED);
+               break;
+            }
+         }
+      }
+      MBXcontrol[0].state = MBXstate_idle;
+      ESCvar.xoe = 0;
+   }
+}

Cards/EaserCAT-2000/Firmware/lib/soes/esc_foe.h

@@ -0,0 +1,77 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+/** \file
+ * \brief
+ * Headerfile for esc_foe.c
+ */
+
+#ifndef __esc_foe__
+#define __esc_foe__
+
+#include <cc.h>
+
+/** Maximum number of characters allowed in a file name. */
+#define FOE_FN_MAX      31
+
+typedef struct foe_file_cfg foe_file_cfg_t;
+struct foe_file_cfg
+{
+   /** Name of file to receive from master */
+   const char * name;
+   /** Size of file,sizeof data we can recv */
+   uint32_t       max_data;
+   /** Where to store the data initially */
+   uint32_t       dest_start_address;
+   /** Current address during write of file */
+   uint32_t       address_offset;
+   /** Calculated size of file received */
+   uint32_t       total_size;
+   /** FoE password */
+   uint32_t       filepass;
+   /** This file can be written only in BOOT state. Intended for FW files */
+   uint8_t        write_only_in_boot;
+   /** for feature use */
+   uint32_t       padding:24;
+   /** Pointer to application foe write function */
+   uint32_t       (*write_function) (foe_file_cfg_t * self, uint8_t * data, size_t length);
+};
+
+typedef struct foe_cfg
+{
+   /** Allocate static in caller func to fit buffer_size */
+   uint8_t * fbuffer;
+   /** Buffer size before we flush to destination */
+   uint32_t  buffer_size;
+   /** Number of files used in firmware update */
+   uint32_t  n_files;
+   /** Pointer to files configured to be used by FoE */
+   foe_file_cfg_t * files;
+} foe_cfg_t;
+
+typedef struct CC_PACKED
+{
+   /** Current FoE state, ex. Waiting for ACK, Waiting for DATA */
+   uint8_t  foestate;
+   /** Current file buffer position, evaluated against foe file buffer size
+    * when to flush
+    */
+   uint16_t fbufposition;
+   /** Frame number in read or write sequence */
+   uint32_t foepacket;
+   /** Current position in file to be handled by FoE request */
+   uint32_t fposition;
+   /** Previous position in file to be handled by FoE request */
+   uint32_t fprevposition;
+   /** End position of allocated disk space for FoE requested file  */
+   uint32_t fend;
+} _FOEvar;
+
+/* Initializes FoE state. */
+void FOE_config (foe_cfg_t * cfg);
+void FOE_init (void);
+void ESC_foeprocess (void);
+
+#endif

Cards/EaserCAT-2000/Firmware/lib/soes/esi.json

@@ -0,0 +1,263 @@
+{
+  "form": {
+    "VendorName": "MetalMusings",
+    "VendorID": "0xaaa",
+    "ProductCode": "0xbbbccc",
+    "ProfileNo": "5001",
+    "RevisionNumber": "0x002",
+    "SerialNumber": "0x001",
+    "HWversion": "0.0.1",
+    "SWversion": "0.0.1",
+    "EEPROMsize": "2048",
+    "RxMailboxOffset": "0x1000",
+    "TxMailboxOffset": "0x1200",
+    "MailboxSize": "512",
+    "SM2Offset": "0x1600",
+    "SM3Offset": "0x1A00",
+    "TextGroupType": "MachineControl",
+    "TextGroupName5": "Incremental encoder",
+    "ImageName": "IMGCBY",
+    "TextDeviceType": "EaserCAT2000",
+    "TextDeviceName": "MetalMusings EaserCAT 2000",
+    "Port0Physical": "Y",
+    "Port1Physical": "Y",
+    "Port2Physical": " ",
+    "Port3Physical": " ",
+    "ESC": "LAN9252",
+    "SPImode": "3",
+    "CoeDetailsEnableSDO": "EnableSDO",
+    "CoeDetailsEnableSDOInfo": "EnableSDOInfo",
+    "CoeDetailsEnablePDOAssign": "EnablePDOAssign",
+    "CoeDetailsEnablePDOConfiguration": "EnablePDOConfiguration",
+    "CoeDetailsEnableUploadAtStartup": "EnableUploadAtStartup",
+    "CoeDetailsEnableSDOCompleteAccess": "EnableSDOCompleteAccess"
+  },
+  "od": {
+    "sdo": {
+      "A": {
+        "otype": "RECORD",
+        "name": "Error Settings",
+        "access": "RO",
+        "items": [
+          {
+            "name": "Max SubIndex"
+          },
+          {
+            "name": "New record subitem",
+            "dtype": "UNSIGNED8"
+          }
+        ]
+      }
+    },
+    "txpdo": {
+      "6000": {
+        "otype": "VAR",
+        "name": "EncPos",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.EncPos"
+      },
+      "6001": {
+        "otype": "VAR",
+        "name": "EncFrequency",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.EncFrequency"
+      },
+      "6002": {
+        "otype": "VAR",
+        "name": "DiffT",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "UNSIGNED16",
+        "value": "0",
+        "data": "&Obj.DiffT"
+      },
+      "6003": {
+        "otype": "VAR",
+        "name": "IndexByte",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "UNSIGNED32",
+        "value": "0",
+        "data": "&Obj.IndexByte"
+      },
+      "6004": {
+        "otype": "VAR",
+        "name": "IndexStatus",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "UNSIGNED32",
+        "value": "0",
+        "data": "&Obj.IndexStatus"
+      },
+      "6005": {
+        "otype": "VAR",
+        "name": "ActualPosition1",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.ActualPosition1"
+      },
+      "6006": {
+        "otype": "VAR",
+        "name": "ActualPosition2",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.ActualPosition2"
+      },
+      "6007": {
+        "otype": "VAR",
+        "name": "D1",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.D1"
+      },
+      "6008": {
+        "otype": "VAR",
+        "name": "D2",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.D2"
+      },
+      "6009": {
+        "otype": "VAR",
+        "name": "D3",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.D3"
+      },
+      "600A": {
+        "otype": "VAR",
+        "name": "D4",
+        "access": "RO",
+        "pdo_mappings": [
+          "txpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.D4"
+      }
+    },
+    "rxpdo": {
+      "7000": {
+        "otype": "VAR",
+        "name": "IndexLatchEnable",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "UNSIGNED32",
+        "value": "0",
+        "data": "&Obj.IndexLatchEnable"
+      },
+      "7001": {
+        "otype": "VAR",
+        "name": "CommandedPosition1",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.CommandedPosition1"
+      },
+      "7002": {
+        "otype": "VAR",
+        "name": "CommandedPosition2",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "REAL32",
+        "value": "0",
+        "data": "&Obj.CommandedPosition2"
+      },
+      "7003": {
+        "otype": "VAR",
+        "name": "StepsPerMM1",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.StepsPerMM1"
+      },
+      "7004": {
+        "otype": "VAR",
+        "name": "StepsPerMM2",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "INTEGER16",
+        "value": "0",
+        "data": "&Obj.StepsPerMM2"
+      },
+      "60664": {
+        "otype": "VAR",
+        "name": "ActualPosition",
+        "access": "RO",
+        "pdo_mappings": [
+          "rxpdo"
+        ],
+        "dtype": "INTEGER32",
+        "value": "0"
+      }
+    }
+  },
+  "dc": [
+    {
+      "Name": "SM-Synchron",
+      "Description": "SM-Synchron",
+      "AssignActivate": "#x000",
+      "Sync0cycleTime": "0",
+      "Sync0shiftTime": "0",
+      "Sync1cycleTime": "0",
+      "Sync1shiftTime": "0"
+    },
+    {
+      "Name": "DC",
+      "Description": "DC-Synchron",
+      "AssignActivate": "#x300",
+      "Sync0cycleTime": "0",
+      "Sync0shiftTime": "0",
+      "Sync1cycleTime": "0",
+      "Sync1shiftTime": "0"
+    }
+  ]
+}

Cards/EaserCAT-2000/Firmware/lib/soes/hal/arduino-lan9252/esc_hw.c

@@ -0,0 +1,458 @@
+/*
+ * Licensed under the GNU General Public License version 2 with exceptions. See
+ * LICENSE file in the project root for full license information
+ */
+
+ /** \file
+ * \brief
+ * ESC hardware layer functions for LAN9252.
+ *
+ * Function to read and write commands to the ESC. Used to read/write ESC
+ * registers and memory.
+ */
+#include "esc.h"
+#include "spi.h"
+#include <string.h>
+
+
+#define O_RDWR 1
+
+#define BIT(x)	1 << (x)
+
+#define ESC_CMD_SERIAL_WRITE     0x02
+#define ESC_CMD_SERIAL_READ      0x03
+#define ESC_CMD_FAST_READ        0x0B
+#define ESC_CMD_RESET_SQI        0xFF
+
+#define ESC_CMD_FAST_READ_DUMMY  1
+#define ESC_CMD_ADDR_INC         BIT(6)
+
+#define ESC_PRAM_RD_FIFO_REG     0x000
+#define ESC_PRAM_WR_FIFO_REG     0x020
+#define ESC_PRAM_RD_ADDR_LEN_REG 0x308
+#define ESC_PRAM_RD_CMD_REG      0x30C
+#define ESC_PRAM_WR_ADDR_LEN_REG 0x310
+#define ESC_PRAM_WR_CMD_REG      0x314
+
+#define ESC_PRAM_CMD_BUSY        BIT(31)
+#define ESC_PRAM_CMD_ABORT       BIT(30)
+
+#define ESC_PRAM_CMD_CNT(x)      ((x >> 8) & 0x1F)
+#define ESC_PRAM_CMD_AVAIL       BIT(0)
+
+#define ESC_PRAM_SIZE(x)         ((x) << 16)
+#define ESC_PRAM_ADDR(x)         ((x) << 0)
+
+#define ESC_CSR_DATA_REG         0x300
+#define ESC_CSR_CMD_REG          0x304
+
+#define ESC_CSR_CMD_BUSY         BIT(31)
+#define ESC_CSR_CMD_READ         (BIT(31) | BIT(30))
+#define ESC_CSR_CMD_WRITE        BIT(31)
+#define ESC_CSR_CMD_SIZE(x)      (x << 16)
+
+#define ESC_RESET_CTRL_REG       0x1F8
+#define ESC_RESET_CTRL_RST       BIT(6)
+#define ESC_DIGITAL_RST          0x00000001
+
+#define ESC_ID_REV_REG           0x050
+#define LAN9252_ID_REV           0x9252
+
+#define ESC_IRQ_CFG_REG          0x054
+#define ESC_INT_EN_REG           0x05C
+#define ESC_BYTE_TEST_REG        0x064
+#define ESC_TEST_VALUE           0x87654321
+
+#define ESC_HW_CFG_REG           0x074
+#define ESC_READY                BIT(27)
+
+
+static int lan9252 = -1;
+
+/* lan9252 singel write */
+static void lan9252_write_32 (uint16_t address, uint32_t val)
+{
+    uint8_t data[7];
+
+    data[0] = ESC_CMD_SERIAL_WRITE;
+    data[1] = ((address >> 8) & 0xFF);
+    data[2] = (address & 0xFF);
+    data[3] = (val & 0xFF);
+    data[4] = ((val >> 8) & 0xFF);
+    data[5] = ((val >> 16) & 0xFF);
+    data[6] = ((val >> 24) & 0xFF);
+
+    /* Select device. */
+    spi_select (lan9252);
+    /* Write data */
+    write (lan9252, data, sizeof(data));
+    /* Un-select device. */
+    spi_unselect (lan9252);
+}
+
+/* lan9252 single read */
+static uint32_t lan9252_read_32 (uint32_t address)
+{
+   uint8_t data[4];
+   uint8_t result[4];
+
+   data[0] = ESC_CMD_FAST_READ;
+   data[1] = ((address >> 8) & 0xFF);
+   data[2] = (address & 0xFF);
+   data[3] = ESC_CMD_FAST_READ_DUMMY;
+
+   /* Select device. */
+   spi_select (lan9252);
+   /* Read data */
+   write (lan9252, data, sizeof(data));
+   read (lan9252, result, sizeof(result));
+   /* Un-select device. */
+   spi_unselect (lan9252);
+
+   return ((result[3] << 24) |
+           (result[2] << 16) |
+           (result[1] << 8) |
+            result[0]);
+}
+
+/* ESC read CSR function */
+static void ESC_read_csr (uint16_t address, void *buf, uint16_t len)
+{
+   uint32_t value;
+
+   value = (ESC_CSR_CMD_READ | ESC_CSR_CMD_SIZE(len) | address);
+   lan9252_write_32(ESC_CSR_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_CSR_CMD_REG);
+   } while(value & ESC_CSR_CMD_BUSY);
+
+   value = lan9252_read_32(ESC_CSR_DATA_REG);
+   memcpy(buf, (uint8_t *)&value, len);
+}
+
+/* ESC write CSR function */
+static void ESC_write_csr (uint16_t address, void *buf, uint16_t len)
+{
+   uint32_t value;
+
+   memcpy((uint8_t*)&value, buf,len);
+   lan9252_write_32(ESC_CSR_DATA_REG, value);
+   value = (ESC_CSR_CMD_WRITE | ESC_CSR_CMD_SIZE(len) | address);
+   lan9252_write_32(ESC_CSR_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_CSR_CMD_REG);
+   } while(value & ESC_CSR_CMD_BUSY);
+}
+
+/* ESC read process data ram function */
+/*static*/ void ESC_read_pram (uint16_t address, void *buf, uint16_t len)
+{
+   uint32_t value;
+   uint8_t * temp_buf = (uint8_t *)buf;
+   uint16_t byte_offset = 0;
+   uint8_t fifo_cnt, first_byte_position, temp_len, data[4];
+
+   value = ESC_PRAM_CMD_ABORT;
+   lan9252_write_32(ESC_PRAM_RD_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_PRAM_RD_CMD_REG);
+   }while(value & ESC_PRAM_CMD_BUSY);
+
+   value = ESC_PRAM_SIZE(len) | ESC_PRAM_ADDR(address);
+   lan9252_write_32(ESC_PRAM_RD_ADDR_LEN_REG, value);
+
+   value = ESC_PRAM_CMD_BUSY;
+   lan9252_write_32(ESC_PRAM_RD_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_PRAM_RD_CMD_REG);
+   }while((value & ESC_PRAM_CMD_AVAIL) == 0);
+
+   /* Fifo count */
+   fifo_cnt = ESC_PRAM_CMD_CNT(value);
+
+   /* Read first value from FIFO */
+   value = lan9252_read_32(ESC_PRAM_RD_FIFO_REG);
+   fifo_cnt--;
+
+   /* Find out first byte position and adjust the copy from that
+    * according to LAN9252 datasheet and MicroChip SDK code
+    */
+   first_byte_position = (address & 0x03);
+   temp_len = ((4 - first_byte_position) > len) ? len : (4 - first_byte_position);
+
+   memcpy(temp_buf ,((uint8_t *)&value + first_byte_position), temp_len);
+   len -= temp_len;
+   byte_offset += temp_len;
+
+   /* Select device. */
+   spi_select (lan9252);
+   /* Send command and address for fifo read */
+   data[0] = ESC_CMD_FAST_READ;
+   data[1] = ((ESC_PRAM_RD_FIFO_REG >> 8) & 0xFF);
+   data[2] = (ESC_PRAM_RD_FIFO_REG & 0xFF);
+   data[3] = ESC_CMD_FAST_READ_DUMMY;
+   write (lan9252, data, sizeof(data));
+
+   /* Continue reading until we have read len */
+   while(len > 0)
+   {
+      temp_len = (len > 4) ? 4: len;
+      /* Always read 4 byte */
+      read (lan9252, (temp_buf + byte_offset), sizeof(uint32_t));
+
+      fifo_cnt--;
+      len -= temp_len;
+      byte_offset += temp_len;
+   }
+   /* Un-select device. */
+   spi_unselect (lan9252);
+}
+
+/* ESC write process data ram function */
+/* static */ void ESC_write_pram (uint16_t address, void *buf, uint16_t len)
+{
+   uint32_t value;
+   uint8_t * temp_buf = (uint8_t *)buf;
+   uint16_t byte_offset = 0;
+   uint8_t fifo_cnt, first_byte_position, temp_len, data[3];
+
+   value = ESC_PRAM_CMD_ABORT;
+   lan9252_write_32(ESC_PRAM_WR_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_PRAM_WR_CMD_REG);
+   }while(value & ESC_PRAM_CMD_BUSY);
+
+   value = ESC_PRAM_SIZE(len) | ESC_PRAM_ADDR(address);
+   lan9252_write_32(ESC_PRAM_WR_ADDR_LEN_REG, value);
+
+   value = ESC_PRAM_CMD_BUSY;
+   lan9252_write_32(ESC_PRAM_WR_CMD_REG, value);
+
+   do
+   {
+      value = lan9252_read_32(ESC_PRAM_WR_CMD_REG);
+   }while((value & ESC_PRAM_CMD_AVAIL) == 0);
+
+   /* Fifo count */
+   fifo_cnt = ESC_PRAM_CMD_CNT(value);
+
+   /* Find out first byte position and adjust the copy from that
+    * according to LAN9252 datasheet
+    */
+   first_byte_position = (address & 0x03);
+   temp_len = ((4 - first_byte_position) > len) ? len : (4 - first_byte_position);
+
+   memcpy(((uint8_t *)&value + first_byte_position), temp_buf, temp_len);
+
+   /* Write first value from FIFO */
+   lan9252_write_32(ESC_PRAM_WR_FIFO_REG, value);
+
+   len -= temp_len;
+   byte_offset += temp_len;
+   fifo_cnt--;
+
+   /* Select device. */
+   spi_select (lan9252);
+   /* Send command and address for incrementing write */
+   data[0] = ESC_CMD_SERIAL_WRITE;
+   data[1] = ((ESC_PRAM_WR_FIFO_REG >> 8) & 0xFF);
+   data[2] = (ESC_PRAM_WR_FIFO_REG & 0xFF);
+   write (lan9252, data, sizeof(data));
+
+   /* Continue reading until we have read len */
+   while(len > 0)
+   {
+      temp_len = (len > 4) ? 4 : len;
+      value = 0;
+      memcpy((uint8_t *)&value, (temp_buf + byte_offset), temp_len);
+      /* Always write 4 byte */
+      write (lan9252, (uint8_t *)&value, sizeof(value));
+
+      fifo_cnt--;
+      len -= temp_len;
+      byte_offset += temp_len;
+   }
+   /* Un-select device. */
+   spi_unselect (lan9252);
+}
+
+
+/** ESC read function used by the Slave stack.
+ *
+ * @param[in]   address     = address of ESC register to read
+ * @param[out]  buf         = pointer to buffer to read in
+ * @param[in]   len         = number of bytes to read
+ */
+void ESC_read (uint16_t address, void *buf, uint16_t len)
+{
+   /* Select Read function depending on address, process data ram or not */
+   if (address >= 0x1000)
+   {
+      ESC_read_pram(address, buf, len);
+   }
+   else
+   {
+      uint16_t size;
+      uint8_t *temp_buf = (uint8_t *)buf;
+
+      while(len > 0)
+      {
+         /* We write maximum 4 bytes at the time */
+         size = (len > 4) ? 4 : len;
+         /* Make size aligned to address according to LAN9252 datasheet
+          * Table 12-14 EtherCAT CSR Address VS size and MicroChip SDK code
+          */
+         /* If we got an odd address size is 1 , 01b 11b is captured */
+         if(address & BIT(0))
+         {
+            size = 1;
+         }
+         /* If address 1xb and size != 1 and 3 , allow size 2 else size 1 */
+         else if (address & BIT(1))
+         {
+            size = (size & BIT(0)) ? 1 : 2;
+         }
+         /* size 3 not valid */
+         else if (size == 3)
+         {
+            size = 1;
+         }
+         /* else size is kept AS IS */
+         ESC_read_csr(address, temp_buf, size);
+
+         /* next address */
+         len -= size;
+         temp_buf += size;
+         address += size;
+      }
+   }
+   /* To mimic the ET1100 always providing AlEvent on every read or write */
+   ESC_read_csr(ESCREG_ALEVENT,(void *)&ESCvar.ALevent,sizeof(ESCvar.ALevent));
+   ESCvar.ALevent = etohs (ESCvar.ALevent);
+
+}
+
+/** ESC write function used by the Slave stack.
+ *
+ * @param[in]   address     = address of ESC register to write
+ * @param[out]  buf         = pointer to buffer to write from
+ * @param[in]   len         = number of bytes to write
+ */
+void ESC_write (uint16_t address, void *buf, uint16_t len)
+{
+   /* Select Write function depending on address, process data ram or not */
+   if (address >= 0x1000)
+   {
+      ESC_write_pram(address, buf, len);
+   }
+   else
+   {
+      uint16_t size;
+      uint8_t *temp_buf = (uint8_t *)buf;
+
+      while(len > 0)
+      {
+         /* We write maximum 4 bytes at the time */
+         size = (len > 4) ? 4 : len;
+         /* Make size aligned to address according to LAN9252 datasheet
+          * Table 12-14 EtherCAT CSR Address VS size  and MicroChip SDK code
+          */
+         /* If we got an odd address size is 1 , 01b 11b is captured */
+         if(address & BIT(0))
+         {
+            size = 1;
+         }
+         /* If address 1xb and size != 1 and 3 , allow size 2 else size 1 */
+         else if (address & BIT(1))
+         {
+            size = (size & BIT(0)) ? 1 : 2;
+         }
+         /* size 3 not valid */
+         else if (size == 3)
+         {
+            size = 1;
+         }
+         /* else size is kept AS IS */
+         ESC_write_csr(address, temp_buf, size);
+
+         /* next address */
+         len -= size;
+         temp_buf += size;
+         address += size;
+      }
+   }
+
+   /* To mimic the ET1x00 always providing AlEvent on every read or write */
+   ESC_read_csr(ESCREG_ALEVENT,(void *)&ESCvar.ALevent,sizeof(ESCvar.ALevent));
+   ESCvar.ALevent = etohs (ESCvar.ALevent);
+}
+
+/* Un-used due to evb-lan9252-digio not havning any possability to
+ * reset except over SPI.
+ */
+void ESC_reset (void)
+{
+
+}
+
+
+uint8_t ESC_IsLAN9252()
+{
+   volatile uint32_t value;
+   uint16_t detectedChip, revision;
+
+   /* Read */
+   value = lan9252_read_32(ESC_ID_REV_REG);
+   detectedChip = value >> 16;
+   revision = value & 0xFF;
+
+   return detectedChip == LAN9252_ID_REV
+          && revision >= 1;
+}
+
+
+void ESC_init (const esc_cfg_t * config)
+{
+   uint32_t value;
+
+   spi_setup();
+
+   /* Reset the ecat core here due to evb-lan9252-digio not having any GPIO
+    * for that purpose.
+    */
+   lan9252_write_32(ESC_RESET_CTRL_REG, ESC_DIGITAL_RST);
+   do
+   {
+      value = lan9252_read_32(ESC_RESET_CTRL_REG);
+   } 
+   while(value & ESC_RESET_CTRL_RST);
+
+   /* Read test register */
+   do
+   {
+      value = lan9252_read_32(ESC_BYTE_TEST_REG);
+   }
+   while (value != ESC_TEST_VALUE);
+
+   /* Check Ready flag */
+   do
+   {
+      value = lan9252_read_32(ESC_HW_CFG_REG);
+   }
+   while ((value & ESC_READY) == 0);
+
+   if(!ESC_IsLAN9252())
+   {
+      while (1);
+   }
+}

Cards/EaserCAT-2000/Firmware/lib/soes/hal/arduino-lan9252/spi.cpp

@@ -0,0 +1,66 @@
+#include "spi.hpp"
+// #include <Arduino.h>
+#include <SPI.h>
+
+
+char SCS = ESC_GPIO_Pin_CS;
+
+
+void spi_setup(void)
+{
+  SPI.begin();    
+  pinMode(SCS, OUTPUT);   
+  spi_unselect(0);
+  delay(100);     
+  SPI.beginTransaction(SPISettings(SPIX_ESC_SPEED, MSBFIRST, SPI_MODE0)); 
+
+}
+
+void spi_select (int8_t board)
+{
+    // Soft CSN
+    #if SCS_ACTIVE_POLARITY == SCS_LOW
+    digitalWrite(SCS, LOW);
+    #endif
+}
+
+void spi_unselect (int8_t board)
+{
+    // Soft CSN
+    #if SCS_ACTIVE_POLARITY == SCS_LOW
+    digitalWrite(SCS, HIGH);
+    #endif
+}
+
+inline static uint8_t spi_transfer_byte(uint8_t byte)
+{
+    return SPI.transfer(byte);
+    // AVR will need handling last byte transfer difference,
+    // but then again they pobably wont even fit EtherCAT stack in RAM
+    // so no need to care for now
+}
+
+void write (int8_t board, uint8_t *data, uint8_t size)
+{
+    for(int i = 0; i < size; ++i)
+    {
+        spi_transfer_byte(data[i]);
+    }
+}
+
+void read (int8_t board, uint8_t *result, uint8_t size)
+{
+	for(int i = 0; i < size; ++i)
+    {
+        result[i] = spi_transfer_byte(DUMMY_BYTE);
+    }
+}
+
+
+void spi_bidirectionally_transfer (int8_t board, uint8_t *result, uint8_t *data, uint8_t size)
+{
+	for(int i = 0; i < size; ++i)
+    {
+        result[i] = spi_transfer_byte(data[i]);
+    }
+}

Cards/EaserCAT-2000/Firmware/lib/soes/hal/arduino-lan9252/spi.h

@@ -0,0 +1,15 @@
+#ifndef SRC_APP_SPI_H_
+#define SRC_APP_SPI_H_
+
+#include <stdint.h>
+
+
+void spi_setup(void);
+void spi_select (int8_t board);
+void spi_unselect (int8_t board);
+void write (int8_t board, uint8_t *data, uint8_t size);
+void read (int8_t board, uint8_t *result, uint8_t size);
+void spi_bidirectionally_transfer (int8_t board, uint8_t *result, uint8_t *data, uint8_t size);
+
+
+#endif /* SRC_APP_SPI_H_ */

Cards/EaserCAT-2000/Firmware/lib/soes/hal/arduino-lan9252/spi.hpp

@@ -0,0 +1,25 @@
+#ifndef SRC_APP_SPI_H_
+#define SRC_APP_SPI_H_
+
+#include <stdint.h>
+
+#define SCS_LOW                           0
+#define SCS_HIGH                          1
+#define SCS_ACTIVE_POLARITY               SCS_LOW
+
+#define SPIX_ESC                          SPI1
+//#define SPIX_ESC_SPEED                    18000000
+#define SPIX_ESC_SPEED                    50000000
+#define ESC_GPIO_Pin_CS                   PC4
+
+#define DUMMY_BYTE 0xFF
+
+extern "C" void spi_setup(void);
+extern "C" void spi_select (int8_t board);
+extern "C" void spi_unselect (int8_t board);
+extern "C" void write (int8_t board, uint8_t *data, uint8_t size);
+extern "C" void read (int8_t board, uint8_t *result, uint8_t size);
+extern "C" void spi_bidirectionally_transfer (int8_t board, uint8_t *result, uint8_t *data, uint8_t size);
+
+
+#endif /* SRC_APP_SPI_H_ */

Cards/EaserCAT-2000/Firmware/lib/soes/objectlist.c

@@ -0,0 +1,351 @@
+#include "esc_coe.h"
+#include "utypes.h"
+#include <stddef.h>
+
+
+static const char acName1000[] = "Device Type";
+static const char acName1008[] = "Device Name";
+static const char acName1009[] = "Hardware Version";
+static const char acName100A[] = "Software Version";
+static const char acName1018[] = "Identity Object";
+static const char acName1018_00[] = "Max SubIndex";
+static const char acName1018_01[] = "Vendor ID";
+static const char acName1018_02[] = "Product Code";
+static const char acName1018_03[] = "Revision Number";
+static const char acName1018_04[] = "Serial Number";
+static const char acName1600[] = "IndexLatchEnable";
+static const char acName1600_00[] = "Max SubIndex";
+static const char acName1600_01[] = "IndexLatchEnable";
+static const char acName1601[] = "CommandedPosition1";
+static const char acName1601_00[] = "Max SubIndex";
+static const char acName1601_01[] = "CommandedPosition1";
+static const char acName1602[] = "CommandedPosition2";
+static const char acName1602_00[] = "Max SubIndex";
+static const char acName1602_01[] = "CommandedPosition2";
+static const char acName1603[] = "StepsPerMM1";
+static const char acName1603_00[] = "Max SubIndex";
+static const char acName1603_01[] = "StepsPerMM1";
+static const char acName1604[] = "StepsPerMM2";
+static const char acName1604_00[] = "Max SubIndex";
+static const char acName1604_01[] = "StepsPerMM2";
+static const char acName1A00[] = "EncPos";
+static const char acName1A00_00[] = "Max SubIndex";
+static const char acName1A00_01[] = "EncPos";
+static const char acName1A01[] = "EncFrequency";
+static const char acName1A01_00[] = "Max SubIndex";
+static const char acName1A01_01[] = "EncFrequency";
+static const char acName1A02[] = "DiffT";
+static const char acName1A02_00[] = "Max SubIndex";
+static const char acName1A02_01[] = "DiffT";
+static const char acName1A03[] = "IndexByte";
+static const char acName1A03_00[] = "Max SubIndex";
+static const char acName1A03_01[] = "IndexByte";
+static const char acName1A04[] = "IndexStatus";
+static const char acName1A04_00[] = "Max SubIndex";
+static const char acName1A04_01[] = "IndexStatus";
+static const char acName1A05[] = "ActualPosition1";
+static const char acName1A05_00[] = "Max SubIndex";
+static const char acName1A05_01[] = "ActualPosition1";
+static const char acName1A06[] = "ActualPosition2";
+static const char acName1A06_00[] = "Max SubIndex";
+static const char acName1A06_01[] = "ActualPosition2";
+static const char acName1A07[] = "D1";
+static const char acName1A07_00[] = "Max SubIndex";
+static const char acName1A07_01[] = "D1";
+static const char acName1A08[] = "D2";
+static const char acName1A08_00[] = "Max SubIndex";
+static const char acName1A08_01[] = "D2";
+static const char acName1A09[] = "D3";
+static const char acName1A09_00[] = "Max SubIndex";
+static const char acName1A09_01[] = "D3";
+static const char acName1A0A[] = "D4";
+static const char acName1A0A_00[] = "Max SubIndex";
+static const char acName1A0A_01[] = "D4";
+static const char acName1C00[] = "Sync Manager Communication Type";
+static const char acName1C00_00[] = "Max SubIndex";
+static const char acName1C00_01[] = "Communications Type SM0";
+static const char acName1C00_02[] = "Communications Type SM1";
+static const char acName1C00_03[] = "Communications Type SM2";
+static const char acName1C00_04[] = "Communications Type SM3";
+static const char acName1C12[] = "Sync Manager 2 PDO Assignment";
+static const char acName1C12_00[] = "Max SubIndex";
+static const char acName1C12_01[] = "PDO Mapping";
+static const char acName1C12_02[] = "PDO Mapping";
+static const char acName1C12_03[] = "PDO Mapping";
+static const char acName1C12_04[] = "PDO Mapping";
+static const char acName1C12_05[] = "PDO Mapping";
+static const char acName1C13[] = "Sync Manager 3 PDO Assignment";
+static const char acName1C13_00[] = "Max SubIndex";
+static const char acName1C13_01[] = "PDO Mapping";
+static const char acName1C13_02[] = "PDO Mapping";
+static const char acName1C13_03[] = "PDO Mapping";
+static const char acName1C13_04[] = "PDO Mapping";
+static const char acName1C13_05[] = "PDO Mapping";
+static const char acName1C13_06[] = "PDO Mapping";
+static const char acName1C13_07[] = "PDO Mapping";
+static const char acName1C13_08[] = "PDO Mapping";
+static const char acName1C13_09[] = "PDO Mapping";
+static const char acName1C13_10[] = "PDO Mapping";
+static const char acName1C13_11[] = "PDO Mapping";
+static const char acName6000[] = "EncPos";
+static const char acName6001[] = "EncFrequency";
+static const char acName6002[] = "DiffT";
+static const char acName6003[] = "IndexByte";
+static const char acName6004[] = "IndexStatus";
+static const char acName6005[] = "ActualPosition1";
+static const char acName6006[] = "ActualPosition2";
+static const char acName6007[] = "D1";
+static const char acName6008[] = "D2";
+static const char acName6009[] = "D3";
+static const char acName600A[] = "D4";
+static const char acName7000[] = "IndexLatchEnable";
+static const char acName7001[] = "CommandedPosition1";
+static const char acName7002[] = "CommandedPosition2";
+static const char acName7003[] = "StepsPerMM1";
+static const char acName7004[] = "StepsPerMM2";
+
+const _objd SDO1000[] =
+{
+  {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1000, 5001, NULL},
+};
+const _objd SDO1008[] =
+{
+  {0x0, DTYPE_VISIBLE_STRING, 208, ATYPE_RO, acName1008, 0, "MetalMusings EaserCAT 2000"},
+};
+const _objd SDO1009[] =
+{
+  {0x0, DTYPE_VISIBLE_STRING, 40, ATYPE_RO, acName1009, 0, "0.0.1"},
+};
+const _objd SDO100A[] =
+{
+  {0x0, DTYPE_VISIBLE_STRING, 40, ATYPE_RO, acName100A, 0, "0.0.1"},
+};
+const _objd SDO1018[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1018_00, 4, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_01, 2730, NULL},
+  {0x02, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_02, 12303564, NULL},
+  {0x03, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_03, 2, NULL},
+  {0x04, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1018_04, 1, &Obj.serial},
+};
+const _objd SDO1600[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1600_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1600_01, 0x70000020, NULL},
+};
+const _objd SDO1601[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1601_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1601_01, 0x70010020, NULL},
+};
+const _objd SDO1602[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1602_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1602_01, 0x70020020, NULL},
+};
+const _objd SDO1603[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1603_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1603_01, 0x70030010, NULL},
+};
+const _objd SDO1604[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1604_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1604_01, 0x70040010, NULL},
+};
+const _objd SDO1A00[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A00_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A00_01, 0x60000020, NULL},
+};
+const _objd SDO1A01[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A01_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A01_01, 0x60010020, NULL},
+};
+const _objd SDO1A02[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A02_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A02_01, 0x60020010, NULL},
+};
+const _objd SDO1A03[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A03_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A03_01, 0x60030020, NULL},
+};
+const _objd SDO1A04[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A04_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A04_01, 0x60040020, NULL},
+};
+const _objd SDO1A05[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A05_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A05_01, 0x60050020, NULL},
+};
+const _objd SDO1A06[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A06_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A06_01, 0x60060020, NULL},
+};
+const _objd SDO1A07[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A07_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A07_01, 0x60070010, NULL},
+};
+const _objd SDO1A08[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A08_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A08_01, 0x60080010, NULL},
+};
+const _objd SDO1A09[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A09_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A09_01, 0x60090010, NULL},
+};
+const _objd SDO1A0A[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1A0A_00, 1, NULL},
+  {0x01, DTYPE_UNSIGNED32, 32, ATYPE_RO, acName1A0A_01, 0x600A0010, NULL},
+};
+const _objd SDO1C00[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_00, 4, NULL},
+  {0x01, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_01, 1, NULL},
+  {0x02, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_02, 2, NULL},
+  {0x03, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_03, 3, NULL},
+  {0x04, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C00_04, 4, NULL},
+};
+const _objd SDO1C12[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C12_00, 5, NULL},
+  {0x01, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_01, 0x1600, NULL},
+  {0x02, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_02, 0x1601, NULL},
+  {0x03, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_03, 0x1602, NULL},
+  {0x04, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_04, 0x1603, NULL},
+  {0x05, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C12_05, 0x1604, NULL},
+};
+const _objd SDO1C13[] =
+{
+  {0x00, DTYPE_UNSIGNED8, 8, ATYPE_RO, acName1C13_00, 11, NULL},
+  {0x01, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_01, 0x1A00, NULL},
+  {0x02, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_02, 0x1A01, NULL},
+  {0x03, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_03, 0x1A02, NULL},
+  {0x04, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_04, 0x1A03, NULL},
+  {0x05, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_05, 0x1A04, NULL},
+  {0x06, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_06, 0x1A05, NULL},
+  {0x07, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_07, 0x1A06, NULL},
+  {0x08, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_08, 0x1A07, NULL},
+  {0x09, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_09, 0x1A08, NULL},
+  {0x0a, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_10, 0x1A09, NULL},
+  {0x0b, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_11, 0x1A0A, NULL},
+};
+const _objd SDO6000[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6000, 0x00000000, &Obj.EncPos},
+};
+const _objd SDO6001[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6001, 0x00000000, &Obj.EncFrequency},
+};
+const _objd SDO6002[] =
+{
+  {0x0, DTYPE_UNSIGNED16, 16, ATYPE_RO | ATYPE_TXPDO, acName6002, 0, &Obj.DiffT},
+};
+const _objd SDO6003[] =
+{
+  {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_TXPDO, acName6003, 0, &Obj.IndexByte},
+};
+const _objd SDO6004[] =
+{
+  {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_TXPDO, acName6004, 0, &Obj.IndexStatus},
+};
+const _objd SDO6005[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6005, 0x00000000, &Obj.ActualPosition1},
+};
+const _objd SDO6006[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_TXPDO, acName6006, 0x00000000, &Obj.ActualPosition2},
+};
+const _objd SDO6007[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6007, 0, &Obj.D1},
+};
+const _objd SDO6008[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6008, 0, &Obj.D2},
+};
+const _objd SDO6009[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName6009, 0, &Obj.D3},
+};
+const _objd SDO600A[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_TXPDO, acName600A, 0, &Obj.D4},
+};
+const _objd SDO7000[] =
+{
+  {0x0, DTYPE_UNSIGNED32, 32, ATYPE_RO | ATYPE_RXPDO, acName7000, 0, &Obj.IndexLatchEnable},
+};
+const _objd SDO7001[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_RXPDO, acName7001, 0x00000000, &Obj.CommandedPosition1},
+};
+const _objd SDO7002[] =
+{
+  {0x0, DTYPE_REAL32, 32, ATYPE_RO | ATYPE_RXPDO, acName7002, 0x00000000, &Obj.CommandedPosition2},
+};
+const _objd SDO7003[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_RXPDO, acName7003, 0, &Obj.StepsPerMM1},
+};
+const _objd SDO7004[] =
+{
+  {0x0, DTYPE_INTEGER16, 16, ATYPE_RO | ATYPE_RXPDO, acName7004, 0, &Obj.StepsPerMM2},
+};
+
+const _objectlist SDOobjects[] =
+{
+  {0x1000, OTYPE_VAR, 0, 0, acName1000, SDO1000},
+  {0x1008, OTYPE_VAR, 0, 0, acName1008, SDO1008},
+  {0x1009, OTYPE_VAR, 0, 0, acName1009, SDO1009},
+  {0x100A, OTYPE_VAR, 0, 0, acName100A, SDO100A},
+  {0x1018, OTYPE_RECORD, 4, 0, acName1018, SDO1018},
+  {0x1600, OTYPE_RECORD, 1, 0, acName1600, SDO1600},
+  {0x1601, OTYPE_RECORD, 1, 0, acName1601, SDO1601},
+  {0x1602, OTYPE_RECORD, 1, 0, acName1602, SDO1602},
+  {0x1603, OTYPE_RECORD, 1, 0, acName1603, SDO1603},
+  {0x1604, OTYPE_RECORD, 1, 0, acName1604, SDO1604},
+  {0x1A00, OTYPE_RECORD, 1, 0, acName1A00, SDO1A00},
+  {0x1A01, OTYPE_RECORD, 1, 0, acName1A01, SDO1A01},
+  {0x1A02, OTYPE_RECORD, 1, 0, acName1A02, SDO1A02},
+  {0x1A03, OTYPE_RECORD, 1, 0, acName1A03, SDO1A03},
+  {0x1A04, OTYPE_RECORD, 1, 0, acName1A04, SDO1A04},
+  {0x1A05, OTYPE_RECORD, 1, 0, acName1A05, SDO1A05},
+  {0x1A06, OTYPE_RECORD, 1, 0, acName1A06, SDO1A06},
+  {0x1A07, OTYPE_RECORD, 1, 0, acName1A07, SDO1A07},
+  {0x1A08, OTYPE_RECORD, 1, 0, acName1A08, SDO1A08},
+  {0x1A09, OTYPE_RECORD, 1, 0, acName1A09, SDO1A09},
+  {0x1A0A, OTYPE_RECORD, 1, 0, acName1A0A, SDO1A0A},
+  {0x1C00, OTYPE_ARRAY, 4, 0, acName1C00, SDO1C00},
+  {0x1C12, OTYPE_ARRAY, 5, 0, acName1C12, SDO1C12},
+  {0x1C13, OTYPE_ARRAY, 11, 0, acName1C13, SDO1C13},
+  {0x6000, OTYPE_VAR, 0, 0, acName6000, SDO6000},
+  {0x6001, OTYPE_VAR, 0, 0, acName6001, SDO6001},
+  {0x6002, OTYPE_VAR, 0, 0, acName6002, SDO6002},
+  {0x6003, OTYPE_VAR, 0, 0, acName6003, SDO6003},
+  {0x6004, OTYPE_VAR, 0, 0, acName6004, SDO6004},
+  {0x6005, OTYPE_VAR, 0, 0, acName6005, SDO6005},
+  {0x6006, OTYPE_VAR, 0, 0, acName6006, SDO6006},
+  {0x6007, OTYPE_VAR, 0, 0, acName6007, SDO6007},
+  {0x6008, OTYPE_VAR, 0, 0, acName6008, SDO6008},
+  {0x6009, OTYPE_VAR, 0, 0, acName6009, SDO6009},
+  {0x600A, OTYPE_VAR, 0, 0, acName600A, SDO600A},
+  {0x7000, OTYPE_VAR, 0, 0, acName7000, SDO7000},
+  {0x7001, OTYPE_VAR, 0, 0, acName7001, SDO7001},
+  {0x7002, OTYPE_VAR, 0, 0, acName7002, SDO7002},
+  {0x7003, OTYPE_VAR, 0, 0, acName7003, SDO7003},
+  {0x7004, OTYPE_VAR, 0, 0, acName7004, SDO7004},
+  {0xffff, 0xff, 0xff, 0xff, NULL, NULL}
+};

Cards/EaserCAT-2000/Firmware/lib/soes/utypes.h

@@ -0,0 +1,40 @@
+#ifndef __UTYPES_H__
+#define __UTYPES_H__
+
+#include "cc.h"
+
+/* Object dictionary storage */
+
+typedef struct
+{
+   /* Identity */
+
+   uint32_t serial;
+
+   /* Inputs */
+
+   float EncPos;
+   float EncFrequency;
+   uint16_t DiffT;
+   uint32_t IndexByte;
+   uint32_t IndexStatus;
+   float ActualPosition1;
+   float ActualPosition2;
+   int16_t D1;
+   int16_t D2;
+   int16_t D3;
+   int16_t D4;
+
+   /* Outputs */
+
+   uint32_t IndexLatchEnable;
+   float CommandedPosition1;
+   float CommandedPosition2;
+   int16_t StepsPerMM1;
+   int16_t StepsPerMM2;
+
+} _Objects;
+
+extern _Objects Obj;
+
+#endif /* __UTYPES_H__ */

Cards/EaserCAT-2000/Firmware/platformio.ini

@@ -0,0 +1,24 @@
+; PlatformIO Project Configuration File
+;
+;   Build options: build flags, source filter
+;   Upload options: custom upload port, speed and extra flags
+;   Library options: dependencies, extra library storages
+;   Advanced options: extra scripting
+;
+; Please visit documentation for the other options and examples
+; https://docs.platformio.org/page/projectconf.html
+
+[env:genericSTM32F407VGT6]
+framework = arduino
+platform = ststm32
+board = genericSTM32F407VGT6
+upload_protocol = stlink
+debug_tool = stlink
+debug_build_flags = -O0 -g -ggdb
+monitor_port = COM7
+monitor_filters = send_on_enter, time, colorize, log2file
+monitor_speed = 115200
+build_flags = -Wl,--no-warn-rwx-segment
+lib_deps = 
+	SPI
+	rlogiacco/CircularBuffer

Cards/EaserCAT-2000/Firmware/src/MyEncoder.cpp

@@ -0,0 +1,99 @@
+#include "MyENcoder.h"
+
+MyEncoder::MyEncoder(TIM_TypeDef *_tim_base, uint8_t _indexPin, void irq(void))
+{
+    tim_base = _tim_base;
+    indexPin = _indexPin;
+    attachInterrupt(digitalPinToInterrupt(indexPin), irq, RISING); // When Index triggered
+    EncoderInit.SetCount(0);
+}
+
+#define ONE_PERIOD 65536
+#define HALF_PERIOD 32768
+
+int64_t MyEncoder::unwrapEncoder(uint16_t in)
+{
+    int32_t c32 = (int32_t)in - HALF_PERIOD;           // remove half period to determine (+/-) sign of the wrap
+    int32_t dif = (c32 - previousEncoderCounterValue); // core concept: prev + (current - prev) = current
+
+    // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
+    int32_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
+    if (dif < -HALF_PERIOD)
+        mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
+
+    int64_t unwrapped = previousEncoderCounterValue + mod_dif;
+    previousEncoderCounterValue = unwrapped; // load previous value
+
+    return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
+}
+
+void MyEncoder::indexPulse(void)
+{
+    if (pleaseZeroTheCounter)
+    {
+        tim_base->CNT = 0;
+        indexPulseFired = 1;
+        Pos.clear();
+        TDelta.clear();
+        pleaseZeroTheCounter = 0;
+    }
+}
+
+uint8_t MyEncoder::indexHappened()
+{
+    if (indexPulseFired)
+    {
+        indexPulseFired = 0;
+        previousEncoderCounterValue = 0;
+        return 1;
+    }
+    return 0;
+}
+
+double MyEncoder::currentPos()
+{
+    curPos = unwrapEncoder(tim_base->CNT) * PosScaleRes;
+    return curPos;
+}
+
+double MyEncoder::frequency(uint64_t time)
+{
+
+    double diffT = 0;
+    double diffPos = 0;
+    double frequency;
+    TDelta.push(time); // Running average over the length of the circular buffer
+    Pos.push(curPos);
+    if (Pos.size() == RINGBUFFERLEN)
+    {
+        diffT = 1.0e-6 * (TDelta.last() - TDelta.first()); // Time is in microseconds
+        diffPos = fabs(Pos.last() - Pos.first());
+        frequency = diffPos / diffT;
+        oldFrequency = frequency;
+        return frequency; // Revolutions per second
+    }
+    else
+        return oldFrequency;
+}
+uint8_t MyEncoder::getIndexState()
+{
+    return digitalRead(indexPin);
+}
+
+void MyEncoder::setScale(double scale)
+{
+    if (CurPosScale != scale && scale != 0)
+    {
+        CurPosScale = scale;
+        PosScaleRes = 1.0 / double(scale);
+    }
+}
+
+void MyEncoder::setLatch(uint8_t latchEnable)
+{
+    if (latchEnable && !oldLatchCEnable) // Should only happen first time IndexCEnable is set
+    {
+        pleaseZeroTheCounter = 1;
+    }
+    oldLatchCEnable = latchEnable;
+}

Cards/EaserCAT-2000/Firmware/src/StepGen.cpp

@@ -0,0 +1,148 @@
+#include <Arduino.h>
+#include <stdio.h>
+#include "StepGen.h"
+
+StepGen::StepGen(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void))
+{
+    timerIsRunning = 0;
+    timerStepPosition = 0;
+    timerStepDirection = 0;
+    timerStepPositionAtEnd = 0;
+    timerNewEndStepPosition = 0;
+    actualPosition = 0;
+    requestedPosition = 0;
+    stepsPerMM = 0;
+    enabled = 0;
+
+    dirPin = _dirPin;
+    stepPin = _stepPin;
+    timerChan = _timerChannel;
+    MyTim = new HardwareTimer(Timer);
+    MyTim->attachInterrupt(irq);
+    pinMode(dirPin, OUTPUT);
+}
+void StepGen::reqPos(double_t pos)
+{
+    requestedPosition = pos;
+}
+double StepGen::reqPos()
+{
+    return requestedPosition;
+}
+void StepGen::actPos(double pos)
+{
+    actualPosition = pos;
+}
+double StepGen::actPos()
+{
+    return actualPosition;
+}
+
+void StepGen::enable(uint8_t yes)
+{
+    enabled = yes;
+}
+
+void StepGen::handleStepper(void)
+{
+    if (!enabled)
+        return;
+    pwmCycleTime = StepGen::sync0CycleTime;
+
+    actPos(timerStepPosition / double(stepsPerMM));
+    double diffPosition = reqPos() - actPos();
+#if 1
+// Wild "tone" kludge. map() function
+#define SPEED_MIN 0.00005
+#define SPEED_MAX 0.0005
+#define FACT_LOW 1.0
+#define FACT_HIGH 20.0
+    if (abs(diffPosition) < SPEED_MIN) // 60 mm/min = 0.001 mm/ms
+    {
+        pwmCycleTime = FACT_LOW * StepGen::sync0CycleTime;
+    }
+    else if (abs(diffPosition) > SPEED_MAX) // 60 mm/min = 0.001 mm/ms
+    {
+        pwmCycleTime = FACT_HIGH * StepGen::sync0CycleTime;
+    }
+    else
+    {
+        pwmCycleTime = (FACT_LOW + (FACT_HIGH - FACT_LOW) * (abs(diffPosition) - SPEED_MIN) / (SPEED_MAX - SPEED_MIN)) * StepGen::sync0CycleTime;
+    }
+#endif
+    uint64_t fre = (abs(diffPosition) * stepsPerMM * 1000000) / pwmCycleTime; // Frequency needed
+    if (fre > maxFreq)                                                        // Only do maxFre
+    {
+        double maxDist = (maxFreq * pwmCycleTime) / (stepsPerMM * 1000000.0) * (diffPosition > 0 ? 1 : -1);
+        reqPos(actPos() + maxDist);
+    }
+    int32_t pulsesAtEndOfCycle = stepsPerMM * reqPos();
+
+    // Will be picked up by the timer_CB and the timer is reloaded, if it runs.
+    timerNewEndStepPosition = pulsesAtEndOfCycle;
+
+    if (!timerIsRunning) // Timer isn't running. Start it here
+    {
+        int32_t steps = pulsesAtEndOfCycle - timerStepPosition; // Pulses to go + or -
+        if (steps != 0)
+        {
+            if (steps > 0)
+            {
+                digitalWrite(dirPin, HIGH);
+                timerStepDirection = 1;
+            }
+            else
+            {
+                digitalWrite(dirPin, LOW);
+                timerStepDirection = -1;
+            }
+            timerStepPositionAtEnd = pulsesAtEndOfCycle; // Current Position
+            float_t freqf = abs(steps) / (pwmCycleTime*1.0e-6);
+            uint32_t freq = uint32_t(freqf);
+            MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
+            MyTim->setOverflow(freq, HERTZ_FORMAT);
+            MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
+            timerIsRunning = 1;
+            MyTim->resume();
+        }
+    }
+}
+void StepGen::timerCB()
+{
+    timerStepPosition += timerStepDirection; // The step that was just completed
+    if (timerNewEndStepPosition != 0)        // Are we going to reload?
+    {
+        // Input for reload is timerNewEndStepPosition
+        // The timer has current position and from this
+        // can set new frequency and new endtarget for steps
+        MyTim->pause(); // We are not at stop, let's stop it. Note stepPin is floating
+        int32_t steps = timerNewEndStepPosition - timerStepPosition;
+        if (steps != 0)
+        {
+            uint8_t sgn = steps > 0 ? HIGH : LOW;
+            digitalWrite(dirPin, sgn);
+            float_t freqf = abs(steps) / float(pwmCycleTime*1.0e-6);
+            uint32_t freq = uint32_t(freqf);
+            timerStepDirection = steps > 0 ? 1 : -1;
+            timerStepPositionAtEnd = timerNewEndStepPosition;
+            timerNewEndStepPosition = 0; // Set to zero to not reload next time
+            MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
+            MyTim->setOverflow(freq, HERTZ_FORMAT);
+            MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50 %
+            MyTim->resume();
+            timerIsRunning = 1;
+        }
+    }
+    if (timerStepPosition == timerStepPositionAtEnd) // Are we finished?
+    {
+        timerIsRunning = 0;
+        MyTim->pause();
+    }
+}
+
+void StepGen::setScale(int16_t spm)
+{
+    stepsPerMM = spm;
+}
+
+uint32_t StepGen::sync0CycleTime = 0;

Cards/EaserCAT-2000/Firmware/src/StepGen2.cpp

@@ -0,0 +1,124 @@
+#include <Arduino.h>
+#include <stdio.h>
+#include "StepGen2.h"
+#include "extend32to64.h"
+
+extern "C"
+{
+#include "esc.h"
+}
+extern extend32to64 longTime;
+
+StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
+{
+    actualPosition = 0;
+    commandedPosition = 0;
+    commandedStepPosition = 0;
+    initialPosition = 0;
+    initialStepPosition = 0;
+    stepsPerMM = 0;
+    enabled = 0;
+
+    dirPin = _dirPin;
+    dirPinName = digitalPinToPinName(dirPin);
+    stepPin = _stepPin;
+    pulseTimerChan = _timerChannel;
+    pulseTimer = new HardwareTimer(Timer);
+    pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
+    pinMode(dirPin, OUTPUT);
+    startTimer = new HardwareTimer(Timer2);
+    startTimer->attachInterrupt(irq2);
+}
+
+uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
+{
+    frequency = 0;
+    nSteps = 0;
+    dbg = 0;
+    if (!enabled) // Just .... don't
+        return updatePos(0);
+
+    commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
+    nSteps = commandedStepPosition - initialStepPosition;
+    if (nSteps == 0) // No movement
+    {
+        return updatePos(1);
+    }
+    lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9; // nLoops is there in case we missed an ethercat cycle. secs
+
+    if (abs(nSteps) < 0)                                                                    // Some small number
+    {                                                                                       //
+        frequency = (abs(nSteps) + 1) / lcncCycleTime;                                      // Distribute steps inside available time
+        Tpulses = abs(nSteps) / frequency;                                                  //
+        Tstartf = (lcncCycleTime - Tpulses) / 2.0;                                          //
+    }                                                                                       //
+    else                                                                                    // Regular step train, up or down
+    {                                                                                       //
+        float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime;                // Straight line equation.  position = kTRAJ x time + mTRAJ
+        float mTRAJ = initialPosition;                                                      // Operating on incoming positions (not steps)
+        if (kTRAJ > 0)                                                                      //
+            Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; // Crossing upwards
+        else                                                                                //
+            Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;     // Crossing downwards
+        frequency = fabs(kTRAJ * stepsPerMM);                                               //
+        Tpulses = abs(nSteps) / frequency;
+    }
+    updatePos(5);
+
+    uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
+    dbg = timeSinceISR;                                                //
+    Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR;        // Have already wasted some time since the irq.
+
+    if (nSteps == 0) // Can do this much earlier, but want some calculated data for debugging
+        return updatePos(1);
+
+    timerFrequency = uint32_t(ceil(frequency));
+    startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
+    startTimer->refresh();
+    startTimer->resume();
+    return 1;
+}
+
+void StepGen2::startTimerCB()
+{
+    startTimer->pause();                                   // Once is enough.
+    digitalWriteFast(dirPinName, nSteps < 0 ? HIGH : LOW); // nSteps negative => decrease, HIGH
+                                                           // There will be a short break here for t2 usecs, in the future.
+
+    timerEndPosition += nSteps;
+    pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
+    pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
+    // pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
+    pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
+    pulseTimer->refresh();
+    pulseTimer->resume();
+}
+
+void StepGen2::pulseTimerCB()
+{
+    int16_t dir = digitalReadFast(dirPinName); //
+    if (dir == HIGH)                           // The step just taken
+        timerPosition--;
+    else
+        timerPosition++;
+    int32_t diffPosition = timerEndPosition - timerPosition; // Same "polarity" as nSteps
+    if (diffPosition == 0)
+        pulseTimer->pause();
+    else
+    {
+        if (diffPosition < 0 && dir == LOW)     // Change direction. Should not end up here, but alas
+            digitalWriteFast(dirPinName, HIGH); // Normal is to be HIGH when decreasing
+        if (diffPosition > 0 && dir == HIGH)    // Change direction. Should not end up here, but alas
+            digitalWriteFast(dirPinName, LOW);  // Normal is to be LOW when increasing
+                                                // Normally nothing is needed
+    }
+}
+
+uint32_t StepGen2::updatePos(uint32_t i)
+{                                                //
+    initialPosition = commandedPosition;         // Save the numeric position for next step
+    initialStepPosition = commandedStepPosition; // also the step we are at}
+    return i;
+}
+
+uint32_t StepGen2::sync0CycleTime = 0;

Cards/EaserCAT-2000/Firmware/src/Stm32F4_Encoder.cpp

@@ -0,0 +1,391 @@
+#include <Stm32F4_Encoder.h>
+
+/*
+   Stm32F4_Encoder.cpp
+    Created on: Nov 20, 2020
+        Author: GoktugH.
+*/
+// TIM2, TIM3, TIM4, TIM8
+Encoder::Encoder()
+{
+    int unit;
+}
+
+// void Encoder::SetCount(enum EncTimer enc, int64_t Counter)
+void Encoder::SetCount(int64_t Counter)
+{
+    tim_base->CNT = Counter;
+}
+// uint16_t Encoder::GetCount(enum EncTimer enc)
+uint16_t Encoder::GetCount()
+{
+    return tim_base->CNT;
+}
+
+void GpioConfigPortA(GPIO_TypeDef *GPIOx)
+{
+
+    uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
+
+    /* ------------------------- Configure the port pins ---------------- */
+    /*-- GPIO Mode Configuration --*/
+    for (pinpos = 0x00; pinpos < 0x10; pinpos++)
+    {
+        pos = ((uint32_t)0x01) << pinpos;
+        /* Get the port pins position */
+        currentpin = (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_6 | GPIO_Pin_7) & pos;
+
+        if (currentpin == pos)
+        {
+            GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
+            GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
+
+            if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
+            {
+                /* Check Speed mode parameters */
+
+                /* Speed mode configuration */
+                GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
+                GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
+
+                /* Check Output mode parameters */
+
+                /* Output mode configuration*/
+                GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
+                GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
+            }
+
+            /* Pull-up Pull down resistor configuration*/
+            GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
+            GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
+        }
+    }
+}
+
+void GpioConfigPortC(GPIO_TypeDef *GPIOx)
+{
+
+    uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
+
+    /* ------------------------- Configure the port pins ---------------- */
+    /*-- GPIO Mode Configuration --*/
+    for (pinpos = 0x00; pinpos < 0x10; pinpos++)
+    {
+        pos = ((uint32_t)0x01) << pinpos;
+        /* Get the port pins position */
+        currentpin = (GPIO_Pin_6 | GPIO_Pin_7) & pos;
+
+        if (currentpin == pos)
+        {
+            GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
+            GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
+
+            if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
+            {
+                /* Check Speed mode parameters */
+
+                /* Speed mode configuration */
+                GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
+                GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
+
+                /* Check Output mode parameters */
+
+                /* Output mode configuration*/
+                GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
+                GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
+            }
+
+            /* Pull-up Pull down resistor configuration*/
+            GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
+            GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
+        }
+    }
+}
+
+void GpioConfigPortD(GPIO_TypeDef *GPIOx)
+{
+
+    uint32_t pinpos = 0x00, pos = 0x00, currentpin = 0x00;
+
+    /* ------------------------- Configure the port pins ---------------- */
+    /*-- GPIO Mode Configuration --*/
+    for (pinpos = 0x00; pinpos < 0x10; pinpos++)
+    {
+        pos = ((uint32_t)0x01) << pinpos;
+        /* Get the port pins position */
+        currentpin = (GPIO_Pin_12 | GPIO_Pin_13) & pos;
+
+        if (currentpin == pos)
+        {
+            GPIOx->MODER &= ~(GPIO_MODER_MODER0 << (pinpos * 2));
+            GPIOx->MODER |= (((uint32_t)GPIO_Mode_AF) << (pinpos * 2));
+
+            if ((GPIO_Mode_AF == GPIO_Mode_OUT) || (GPIO_Mode_AF == GPIO_Mode_AF))
+            {
+                /* Check Speed mode parameters */
+
+                /* Speed mode configuration */
+                GPIOx->OSPEEDR &= ~(GPIO_OSPEEDER_OSPEEDR0 << (pinpos * 2));
+                GPIOx->OSPEEDR |= ((uint32_t)(GPIO_Speed_50MHz) << (pinpos * 2));
+
+                /* Check Output mode parameters */
+
+                /* Output mode configuration*/
+                GPIOx->OTYPER &= ~((GPIO_OTYPER_OT_0) << ((uint16_t)pinpos));
+                GPIOx->OTYPER |= (uint16_t)(((uint16_t)GPIO_OType_PP) << ((uint16_t)pinpos));
+            }
+
+            /* Pull-up Pull down resistor configuration*/
+            GPIOx->PUPDR &= ~(GPIO_PUPDR_PUPDR0 << ((uint16_t)pinpos * 2));
+            GPIOx->PUPDR |= (((uint32_t)GPIO_PuPd_NOPULL) << (pinpos * 2));
+        }
+    }
+}
+
+void TIM_EncoderInterConfig(TIM_TypeDef *TIMx, uint16_t TIM_EncoderMode, uint16_t TIM_IC1Polarity, uint16_t TIM_IC2Polarity)
+{
+
+    uint16_t tmpsmcr = 0;
+    uint16_t tmpccmr1 = 0;
+    uint16_t tmpccer = 0;
+
+    /* Get the TIMx SMCR register value */
+    tmpsmcr = TIMx->SMCR;
+
+    /* Get the TIMx CCMR1 register value */
+    tmpccmr1 = TIMx->CCMR1;
+
+    /* Get the TIMx CCER register value */
+    tmpccer = TIMx->CCER;
+
+    /* Set the encoder Mode */
+    tmpsmcr &= (uint16_t)~TIM_SMCR_SMS;
+    tmpsmcr |= TIM_EncoderMode;
+
+    /* Select the Capture Compare 1 and the Capture Compare 2 as input */
+    tmpccmr1 &= ((uint16_t)~TIM_CCMR1_CC1S) & ((uint16_t)~TIM_CCMR1_CC2S);
+    tmpccmr1 |= TIM_CCMR1_CC1S_0 | TIM_CCMR1_CC2S_0;
+
+    /* Set the TI1 and the TI2 Polarities */
+    tmpccer &= ((uint16_t)~TIM_CCER_CC1P) & ((uint16_t)~TIM_CCER_CC2P);
+    tmpccer |= (uint16_t)(TIM_IC1Polarity | (uint16_t)(TIM_IC2Polarity << (uint16_t)4));
+
+    /* Write to TIMx SMCR */
+    TIMx->SMCR = tmpsmcr;
+
+    /* Write to TIMx CCMR1 */
+    TIMx->CCMR1 = tmpccmr1;
+
+    /* Write to TIMx CCER */
+    TIMx->CCER = tmpccer;
+}
+
+void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct)
+{
+    /* Set the default configuration */
+    TIM_TimeBaseInitStruct->TIM_Period = 0xFFFFFFFF;
+    TIM_TimeBaseInitStruct->TIM_Prescaler = 0x0000;
+    TIM_TimeBaseInitStruct->TIM_ClockDivision = TIM_CKD_DIV1;
+    TIM_TimeBaseInitStruct->TIM_CounterMode = TIM_CounterMode_Up;
+    TIM_TimeBaseInitStruct->TIM_RepetitionCounter = 0x0000;
+}
+
+void TIM_TimeBaseInit(TIM_TypeDef *TIMx, TIM_TimeBaseInitTypeDef *TIM_TimeBaseInitStruct)
+{
+    uint16_t tmpcr1 = 0;
+
+    tmpcr1 = TIMx->CR1;
+
+    if ((TIMx == TIM1) || (TIMx == TIM8) ||
+        (TIMx == TIM2) || (TIMx == TIM3) ||
+        (TIMx == TIM4) || (TIMx == TIM5))
+    {
+        /* Select the Counter Mode */
+        tmpcr1 &= (uint16_t)(~(TIM_CR1_DIR | TIM_CR1_CMS));
+        tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_CounterMode;
+    }
+
+    if ((TIMx != TIM6) && (TIMx != TIM7))
+    {
+        /* Set the clock division */
+        tmpcr1 &= (uint16_t)(~TIM_CR1_CKD);
+        tmpcr1 |= (uint32_t)TIM_TimeBaseInitStruct->TIM_ClockDivision;
+    }
+
+    TIMx->CR1 = tmpcr1;
+
+    /* Set the Autoreload value */
+    TIMx->ARR = TIM_TimeBaseInitStruct->TIM_Period;
+
+    /* Set the Prescaler value */
+    TIMx->PSC = TIM_TimeBaseInitStruct->TIM_Prescaler;
+
+    if ((TIMx == TIM1) || (TIMx == TIM8))
+    {
+        /* Set the Repetition Counter value */
+        TIMx->RCR = TIM_TimeBaseInitStruct->TIM_RepetitionCounter;
+    }
+
+    /* Generate an update event to reload the Prescaler
+       and the repetition counter(only for TIM1 and TIM8) value immediatly */
+    TIMx->EGR = TIM_PSCReloadMode_Immediate;
+}
+
+TIM_TimeBaseInitTypeDef TIMER_InitStructure;
+TIM_TimeBaseInitTypeDef TIMER_InitStructureE;
+TIM_TimeBaseInitTypeDef TIMER_InitStructureEE;
+TIM_TimeBaseInitTypeDef TIMER_InitStructureEEG;
+
+void TIM_Cmd(TIM_TypeDef *TIMx, FunctionalState NewState)
+{
+
+    if (NewState != DISABLE)
+    {
+        /* Enable the TIM Counter */
+        TIMx->CR1 |= TIM_CR1_CEN;
+    }
+    else
+    {
+        /* Disable the TIM Counter */
+        TIMx->CR1 &= (uint16_t)~TIM_CR1_CEN;
+    }
+}
+void GPIO_PinAF(GPIO_TypeDef *GPIOx, uint16_t GPIO_PinSource, uint8_t GPIO_AF)
+
+{
+
+    uint32_t temp = 0x00;
+    uint32_t temp_2 = 0x00;
+
+    temp = ((uint32_t)(GPIO_AF) << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
+    GPIOx->AFR[GPIO_PinSource >> 0x03] &= ~((uint32_t)0xF << ((uint32_t)((uint32_t)GPIO_PinSource & (uint32_t)0x07) * 4));
+    temp_2 = GPIOx->AFR[GPIO_PinSource >> 0x03] | temp;
+    GPIOx->AFR[GPIO_PinSource >> 0x03] = temp_2;
+}
+
+void encoder_config()
+{
+    RCC->AHB1ENR |= 0x1;  // GPIOA
+    RCC->AHB1ENR |= 0x4;  // GPIOC
+    RCC->AHB1ENR |= 0x8;  // GPIOD
+    RCC->AHB1ENR |= 0x10; // GPIOE
+
+    RCC->APB1ENR |= 0x20000000; // ENABLE DAC
+                                //  RCC->APB2ENR |= 0x00000002; // APB2 TIM8
+    RCC->APB1ENR |= 0x00000004; // APB1 TIM4
+    RCC->APB1ENR |= 0x00000001; // APB1 TIM2
+                                //   RCC->APB1ENR |= 0x00000002; // APB1 TIM3
+
+    GpioConfigPortA(GPIOA);
+    //   GpioConfigPortC(GPIOC);
+    GpioConfigPortD(GPIOD);
+#if 0 // Skipping since TIM8 is step generator and TIM3, chan4 is smae as TIM8, chan4
+    GPIO_PinAF(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3);
+    GPIO_PinAF(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3);
+ 
+    GPIO_PinAF(GPIOC, GPIO_PinSource6, GPIO_AF_TIM8);
+    GPIO_PinAF(GPIOC, GPIO_PinSource7, GPIO_AF_TIM8);
+#endif
+    GPIO_PinAF(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
+    GPIO_PinAF(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
+
+    GPIO_PinAF(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
+    GPIO_PinAF(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
+#if 0 // Skipping since I use TIM8 as stepper generator
+    TIM_EncoderInterConfig(TIM8, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
+    TIMER_InitStructure.TIM_Period = 65535;
+    TIMER_InitStructure.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
+    TIM_TimeBaseInit(TIM8, &TIMER_InitStructure);
+    TIM_TimeBaseStructInit(&TIMER_InitStructure);
+    TIM_Cmd(TIM8, ENABLE);
+    TIM8->CNT = 0;
+#endif
+    TIM_EncoderInterConfig(TIM4, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
+    TIMER_InitStructureE.TIM_Period = 65535;
+    TIMER_InitStructureE.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
+    TIM_TimeBaseInit(TIM4, &TIMER_InitStructureE);
+    TIM_TimeBaseStructInit(&TIMER_InitStructureE);
+    TIM_Cmd(TIM4, ENABLE);
+    TIM4->CNT = 0;
+
+    TIM_EncoderInterConfig(TIM2, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
+    TIMER_InitStructureEE.TIM_Period = 65535;
+    TIMER_InitStructureEE.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
+    TIM_TimeBaseInit(TIM2, &TIMER_InitStructureEE);
+    TIM_TimeBaseStructInit(&TIMER_InitStructureEE);
+    TIM_Cmd(TIM2, ENABLE);
+
+    TIM2->CNT = 0;
+#if 0
+    TIM_EncoderInterConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);
+    TIMER_InitStructureEEG.TIM_Period = 65535;
+    TIMER_InitStructureEEG.TIM_CounterMode = TIM_CounterMode_Up | TIM_CounterMode_Down;
+    TIM_TimeBaseInit(TIM3, &TIMER_InitStructureEEG);
+    TIM_TimeBaseStructInit(&TIMER_InitStructureEEG);
+    TIM_Cmd(TIM3, ENABLE);
+
+    TIM3->CNT = 0;
+#endif
+}
+
+void encoder2_config()
+{
+#if 0
+#include "mbed.h"
+#include "stm32f4xx.h"
+#include "stm32f4xx_hal_tim_ex.h"
+
+    TIM_HandleTypeDef timer;
+    TIM_Encoder_InitTypeDef encoder;
+
+    // direction to PA_9 -- step pulse to PA_8
+
+    int main()
+    {
+
+     GPIO_InitTypeDef GPIO_InitStruct;
+        __TIM1_CLK_ENABLE();
+        __GPIOA_CLK_ENABLE();
+        GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9;
+        GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+        GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+        GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+        GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
+        HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    timer.Instance = TIM1;
+    timer.Init.Period = 0xffff;
+    timer.Init.Prescaler = 0;
+    timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    timer.Init.CounterMode = TIM_COUNTERMODE_UP;
+
+    encoder.EncoderMode = TIM_ENCODERMODE_TI12; 
+    encoder.IC1Filter = 0x0f;
+    encoder.IC1Polarity = TIM_INPUTCHANNELPOLARITY_RISING; 
+    encoder.IC1Prescaler = TIM_ICPSC_DIV4;
+    encoder.IC1Selection = TIM_ICSELECTION_DIRECTTI;
+
+    encoder.IC2Filter = 0x0f;
+    encoder.IC2Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;    
+    encoder.IC2Prescaler = TIM_ICPSC_DIV4;
+    encoder.IC2Selection = TIM_ICSELECTION_DIRECTTI;
+
+    HAL_TIM_Encoder_Init(&timer, &encoder);
+    HAL_TIM_Encoder_Start(&timer,TIM_CHANNEL_1);   
+
+
+    TIM1->EGR = 1;           // Generate an update event
+    TIM1->CR1 = 1;           // Enable the counter
+
+
+ while (1) {
+        int16_t count1; 
+        count1=TIM1->CNT; 
+
+        printf("%d\r\n", count1);
+        wait(1.0);
+
+ };
+}
+#endif
+}

Cards/EaserCAT-2000/Firmware/src/extend32to64.cpp

@@ -0,0 +1,18 @@
+#include "extend32to64.h"
+
+// Extend from 32-bit to 64-bit precision
+int64_t extend32to64::extendTime(uint32_t in)
+{
+    int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
+    int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current
+
+    // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
+    int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
+    if (dif < int64_t(-HALF_PERIOD))
+        mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
+
+    int64_t unwrapped = previousTimeValue + mod_dif;
+    previousTimeValue = unwrapped; // load previous value
+
+    return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
+}

Cards/EaserCAT-2000/Firmware/src/main.cpp

@@ -0,0 +1,355 @@
+#include <Arduino.h>
+#include <stdio.h>
+extern "C"
+{
+#include "ecat_slv.h"
+#include "utypes.h"
+};
+_Objects Obj;
+
+HardwareSerial Serial1(PA10, PA9);
+
+volatile uint16_t ALEventIRQ; // ALEvent that caused the interrupt
+HardwareTimer *baseTimer;     // The base period timer
+HardwareTimer *syncTimer;     // The timer that syncs "with linuxcnc cycle"
+uint32_t sync0CycleTime;      // nanosecs, often 1000000 ( 1 ms )
+
+#include "MyEncoder.h"
+volatile uint16_t encCnt = 0;
+void indexPulseEncoderCB1(void);
+MyEncoder Encoder1(TIM2, PA2, indexPulseEncoderCB1);
+void indexPulseEncoderCB1(void)
+{
+   encCnt++;
+   Encoder1.indexPulse();
+}
+// #include <RunningAverage.h>
+// RunningAverage irqServeDelays(1000); // To get the max delay of the irq serve time over the last second
+CircularBuffer<uint16_t, 1000> irqServeDelays;
+
+#include "StepGen3.h"
+StepGen3 *Step = 0;
+
+#include "extend32to64.h"
+
+volatile uint64_t irqTime = 0, irqCnt = 0;
+extend32to64 longTime;
+
+void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs);
+
+void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
+{
+   Encoder1.setLatch(Obj.IndexLatchEnable);
+   Encoder1.setScale(2000);
+}
+
+volatile uint16_t basePeriodCnt;
+volatile uint64_t makePulsesCnt = 0, prevMakePulsesCnt = 0;
+volatile uint16_t deltaMakePulsesCnt;
+
+volatile double posCmd1, posCmd2;
+double oldPosCmd1, oldPosCmd2;
+double deltaPosCmd1, deltaPosCmd2;
+
+void syncWithLCNC()
+{
+   syncTimer->pause();
+   baseTimer->pause();
+   deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt;
+   prevMakePulsesCnt = makePulsesCnt;
+   Step->updateStepGen(posCmd1, posCmd2, sync0CycleTime); // Update positions
+   Step->makeAllPulses();                                 // Make first step right here
+   basePeriodCnt = sync0CycleTime / BASE_PERIOD;          //
+   baseTimer->refresh();                                  //
+   baseTimer->resume();                                   // Make the other steps in ISR
+}
+
+void basePeriodCB(void)
+{
+   if (--basePeriodCnt > 0) // Stop
+      Step->makeAllPulses();
+   else
+      baseTimer->pause();
+}
+
+int32_t delayT;
+uint16_t thisCycleTime; // In usecs
+int16_t maxIrqServeTime = 0;
+uint64_t oldIrqTime = 0;
+uint16_t nLoops;
+uint16_t failedSM2s = 0;
+uint16_t totalFailedSM2s = 0;
+uint16_t nLoopsAboveNorm = 0;
+
+void handleStepper(void)
+{
+   if (oldIrqTime != 0) // See if there is a delay in data, normally it *should* be nLoops=1, but sometimes it is more
+   {
+      thisCycleTime = irqTime - oldIrqTime;
+      nLoops = round(float(thisCycleTime) / float(sync0CycleTime / 1000));
+      nLoopsAboveNorm += nLoops - 1;
+   }
+   oldIrqTime = irqTime;
+
+   uint32_t diffT = longTime.extendTime(micros()) - irqTime; // Time from interrupt was received by isr
+   irqServeDelays.push(diffT);
+   if (irqServeDelays.isFull()) // Do max calcs, just waiting a second
+   {
+      uint16_t maxInBuffer = 0;
+      using index_t = decltype(irqServeDelays)::index_t;
+      for (index_t i = 0; i < irqServeDelays.size(); i++)
+      {
+         if (maxInBuffer < irqServeDelays[i])
+            maxInBuffer = irqServeDelays[i];
+      }
+      if (maxIrqServeTime > maxInBuffer) // Reduce by one, slowly eating up excess time
+         maxIrqServeTime--;
+      if (maxIrqServeTime < maxInBuffer)
+         maxIrqServeTime = maxInBuffer;
+   }
+   if (ALEventIRQ & ESCREG_ALEVENT_SM2)
+   {                                    // The normal case, position update every cycle
+      posCmd1 = Obj.CommandedPosition1; // The position update
+      posCmd2 = Obj.CommandedPosition2;
+      deltaPosCmd1 = posCmd1 - oldPosCmd1; // Needed for extrapolation in the other case
+      deltaPosCmd2 = posCmd2 - oldPosCmd2;
+      failedSM2s = 0;
+   }
+   else
+   {                              // Not normal, we didn't get a position update. Extrapolate from previous updates
+      if (failedSM2s++ < 100)      // Do max 10 such extrapolations, should be plenty
+      {                           //
+         posCmd1 += deltaPosCmd1; // Continue with the same speed
+         posCmd2 += deltaPosCmd2;
+      }
+      totalFailedSM2s++;
+   }
+   oldPosCmd1 = posCmd1;
+   oldPosCmd2 = posCmd2;
+
+   // Obj.ActualPosition1 = Obj.CommandedPosition1;
+   // Obj.ActualPosition2 = Obj.CommandedPosition2;
+
+   Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
+   Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
+
+   Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
+   Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
+
+   delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin
+   if (delayT > 0 && delayT < 900)
+   {
+      syncTimer->setOverflow(delayT, MICROSEC_FORMAT); // Work in flawed units, its ok
+      syncTimer->refresh();
+      syncTimer->resume();
+   }
+   else
+   {
+      syncWithLCNC();
+   }
+}
+
+float_t oldCommandedPosition = 0;
+void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
+{
+   Obj.IndexStatus = Encoder1.indexHappened();
+   Obj.EncPos = Encoder1.currentPos();
+   Obj.EncFrequency = Encoder1.frequency(longTime.extendTime(micros()));
+   Obj.IndexByte = Encoder1.getIndexState();
+
+   Obj.DiffT = nLoops;
+   Obj.D1 = 1000 * deltaPosCmd2;           // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
+   Obj.D2 = totalFailedSM2s;               // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX;                          // Step->stepgen_array[1].freq;
+   Obj.D3 = 1000 * Obj.CommandedPosition2; // Step->stepgen_array[1].freq;
+   Obj.D4 = 1000 * posCmd2;                // Step->stepgen_array[1].rawcount % UINT16_MAX;
+   oldCommandedPosition = Obj.CommandedPosition2;
+}
+
+void ESC_interrupt_enable(uint32_t mask);
+void ESC_interrupt_disable(uint32_t mask);
+uint16_t dc_checker(void);
+void sync0Handler(void);
+
+static esc_cfg_t config =
+    {
+        .user_arg = NULL,
+        .use_interrupt = 1,
+        .watchdog_cnt = 150,
+        .set_defaults_hook = NULL,
+        .pre_state_change_hook = NULL,
+        .post_state_change_hook = NULL,
+        .application_hook = handleStepper,
+        .safeoutput_override = NULL,
+        .pre_object_download_hook = NULL,
+        .post_object_download_hook = NULL,
+        .rxpdo_override = NULL,
+        .txpdo_override = NULL,
+        .esc_hw_interrupt_enable = ESC_interrupt_enable,
+        .esc_hw_interrupt_disable = ESC_interrupt_disable,
+        .esc_hw_eep_handler = NULL,
+        .esc_check_dc_handler = dc_checker,
+};
+
+void measureCrystalFrequency(void);
+
+volatile byte serveIRQ = 0;
+
+void setup(void)
+{
+   Serial1.begin(115200);
+#if 0
+   measureCrystalFrequency(); // Calibrate crystal frequency
+#endif
+   Step = new StepGen3;
+
+   encoder_config(); // Needed by encoder, probably breaks some timers.
+   ecat_slv_init(&config);
+
+   pinMode(PA11, OUTPUT); // Step X
+   pinMode(PA12, OUTPUT); // Dir X
+   pinMode(PC9, OUTPUT);  // Step Z
+   pinMode(PC10, OUTPUT); // Dir Z
+
+   baseTimer = new HardwareTimer(TIM11); // The base period timer
+   setFrequencyAdjustedMicrosSeconds(baseTimer, BASE_PERIOD / 1000);
+   // baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); // Or the line above, This one is uncalibrated
+   baseTimer->attachInterrupt(basePeriodCB);
+
+   syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer
+   syncTimer->attachInterrupt(syncWithLCNC);
+}
+
+void loop(void)
+{
+   uint64_t dTime;
+   if (serveIRQ)
+   {
+      DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
+                                  DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
+      serveIRQ = 0;
+      ESCvar.PrevTime = ESCvar.Time;
+      ecat_slv_poll();
+   }
+   dTime = longTime.extendTime(micros()) - irqTime;
+   if (dTime > 5000) // Don't run ecat_slv_poll when expecting to serve interrupt
+      ecat_slv_poll();
+}
+
+void sync0Handler(void)
+{
+   ALEventIRQ = ESC_ALeventread();
+   // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
+   {
+      irqTime = longTime.extendTime(micros());
+      serveIRQ = 1;
+   }
+   irqCnt++; // debug output
+}
+
+// Enable SM2 interrupts
+void ESC_interrupt_enable(uint32_t mask)
+{
+   // Enable interrupt for SYNC0 or SM2 or SM3
+   // uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3;
+   uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2
+   if (mask & user_int_mask)
+   {
+      ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask));
+      ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3));
+      attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING);
+
+      // Set LAN9252 interrupt pin driver as push-pull active high
+      uint32_t bits = 0x00000111;
+      ESC_write(0x54, &bits, 4);
+
+      // Enable LAN9252 interrupt
+      bits = 0x00000001;
+      ESC_write(0x5c, &bits, 4);
+   }
+}
+
+// Disable SM2 interrupts
+void ESC_interrupt_disable(uint32_t mask)
+{
+   // Enable interrupt for SYNC0 or SM2 or SM3
+   // uint32_t user_int_mask = ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM2 | ESCREG_ALEVENT_SM3;
+   uint32_t user_int_mask = ESCREG_ALEVENT_SM2;
+
+   if (mask & user_int_mask)
+   {
+      // Disable interrupt from SYNC0
+      ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(mask & user_int_mask));
+      detachInterrupt(digitalPinToInterrupt(PC3));
+      // Disable LAN9252 interrupt
+      uint32_t bits = 0x00000000;
+      ESC_write(0x5c, &bits, 4);
+   }
+}
+
+extern "C" uint32_t ESC_SYNC0cycletime(void);
+
+// Setup of DC
+uint16_t dc_checker(void)
+{
+   // Indicate we run DC
+   ESCvar.dcsync = 1;
+   sync0CycleTime = ESC_SYNC0cycletime(); // nanosecs
+   return 0;
+}
+
+//
+// Code to calibrate the crystal.
+//
+
+#include <HardwareTimer.h>
+HardwareTimer *timer;
+volatile uint32_t cnt;
+
+void CB(void)
+{
+   if (cnt-- == 0)
+   {
+      timer->pause();
+   }
+}
+
+void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs)
+{
+   const uint16_t calibrated1000 = 1042;                                               // <- This is the factor to adjust to make 1 sec = 1 sec
+   uint32_t period_cyc = (usecs * (timer->getTimerClkFreq() / 1000)) / calibrated1000; // Avoid overflow during math
+   uint32_t Prescalerfactor = (period_cyc / 0x10000) + 1;
+   uint32_t PeriodTicks = period_cyc / Prescalerfactor;
+   timer->setPrescaleFactor(Prescalerfactor);
+   timer->setOverflow(PeriodTicks, TICK_FORMAT);
+   // Serial1.printf("Period_cyc=%u Prescalefactor =%u ticks = %u\n", period_cyc, Prescalerfactor, PeriodTicks);
+}
+
+void measureCrystalFrequency(void)
+{
+   timer = new HardwareTimer(TIM1);
+   Serial1.begin(115200);
+   delay(3000);
+   Serial1.printf("Clock freq = %u\n", timer->getTimerClkFreq());
+   setFrequencyAdjustedMicrosSeconds(timer, 1000);
+   timer->refresh();
+   timer->attachInterrupt(CB);
+   cnt = 10000;
+
+   Serial1.printf("\n");
+   uint32_t startT = micros();
+   timer->resume();
+
+   while (cnt != 0)
+      ;
+
+   uint32_t endT = micros();
+   uint32_t diffT = endT - startT;
+   Serial1.printf("\n");
+   Serial1.printf("diff = %u\n", diffT);
+
+   Serial1.printf("\n");
+   delay(10000);
+   Serial1.printf("\n");
+
+   exit(0);
+}

Cards/EaserCAT-2000/Firmware/test/README

@@ -0,0 +1,11 @@
+
+This directory is intended for PlatformIO Test Runner and project tests.
+
+Unit Testing is a software testing method by which individual units of
+source code, sets of one or more MCU program modules together with associated
+control data, usage procedures, and operating procedures, are tested to
+determine whether they are fit for use. Unit testing finds problems early
+in the development cycle.
+
+More information about PlatformIO Unit Testing:
+- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html