Browse Source

Add reference C implementations for MultiU16X8toH16/MultiU24X24toH16

Higher step counts might still overflow the ASM MultiU24X24toH16.

https://github.com/MarlinFirmware/Marlin/commit/e4595fa24a2051e95df8a4ed044d2c94cca18e63
Yuri D'Elia 3 years ago
parent
commit
b8e8f182ca
1 changed files with 11 additions and 5 deletions
  1. 11 5
      Firmware/speed_lookuptable.h

+ 11 - 5
Firmware/speed_lookuptable.h

@@ -80,15 +80,21 @@ asm volatile ( \
 
 #else //_NO_ASM
 
-// NOTE: currently not implemented
-void MultiU16X8toH16(unsigned short& intRes, unsigned char& charIn1, unsigned short& intIn2);
-void MultiU24X24toH16(uint16_t& intRes, int32_t& longIn1, long& longIn2);
+static inline void MultiU16X8toH16(uint16_t& intRes, uint8_t& charIn1, uint16_t& intIn2)
+{
+    intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 16;
+}
+
+static inline void MultiU24X24toH16(uint16_t& intRes, uint32_t& longIn1, uint32_t& longIn2)
+{
+    intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
+}
 
 #endif //_NO_ASM
 
 
 FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops) {
-  unsigned short timer;
+  uint16_t timer;
   if(step_rate > MAX_STEP_FREQUENCY) step_rate = MAX_STEP_FREQUENCY;
 
   if(step_rate > 20000) { // If steprate > 20kHz >> step 4 times
@@ -108,7 +114,7 @@ FORCE_INLINE unsigned short calc_timer(uint16_t step_rate, uint8_t& step_loops)
   if(step_rate >= (8*256)){ // higher step rate
     unsigned short table_address = (unsigned short)&speed_lookuptable_fast[(unsigned char)(step_rate>>8)][0];
     unsigned char tmp_step_rate = (step_rate & 0x00ff);
-    unsigned short gain = (unsigned short)pgm_read_word_near(table_address+2);
+    uint16_t gain = (uint16_t)pgm_read_word_near(table_address+2);
     MultiU16X8toH16(timer, tmp_step_rate, gain);
     timer = (unsigned short)pgm_read_word_near(table_address) - timer;
   }