80 lines
2.1 KiB
C++
80 lines
2.1 KiB
C++
/// @file math.h
|
|
#pragma once
|
|
#include "../config/config.h"
|
|
|
|
namespace modules {
|
|
|
|
/// Specialized math operations
|
|
namespace math {
|
|
|
|
/// (intIn1 * intIn2) >> 8
|
|
static inline uint16_t mulU8X16toH16(const uint8_t charIn1, const uint16_t intIn2) {
|
|
uint16_t intRes;
|
|
#if !defined(__AVR__) || defined(NO_ASM)
|
|
intRes = ((uint32_t)charIn1 * (uint32_t)intIn2) >> 8;
|
|
#else
|
|
asm volatile(
|
|
"clr r26 \n\t"
|
|
"mul %A1, %B2 \n\t"
|
|
"movw %A0, r0 \n\t"
|
|
"mul %A1, %A2 \n\t"
|
|
"add %A0, r1 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"lsr r0 \n\t"
|
|
"adc %A0, r26 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"clr r1 \n\t"
|
|
: "=&r"(intRes)
|
|
: "d"(charIn1), "d"(intIn2)
|
|
: "r26");
|
|
#endif
|
|
return intRes;
|
|
}
|
|
|
|
/// (longIn1 * longIn2) >> 24
|
|
static inline uint16_t mulU24X24toH16(const uint32_t &longIn1, const uint32_t &longIn2) {
|
|
uint16_t intRes;
|
|
#if !defined(__AVR__) || defined(NO_ASM)
|
|
intRes = ((uint64_t)longIn1 * (uint64_t)longIn2) >> 24;
|
|
#else
|
|
asm volatile(
|
|
"clr r26 \n\t"
|
|
"mul %A1, %B2 \n\t"
|
|
"mov r27, r1 \n\t"
|
|
"mul %B1, %C2 \n\t"
|
|
"movw %A0, r0 \n\t"
|
|
"mul %C1, %C2 \n\t"
|
|
"add %B0, r0 \n\t"
|
|
"mul %C1, %B2 \n\t"
|
|
"add %A0, r0 \n\t"
|
|
"adc %B0, r1 \n\t"
|
|
"mul %A1, %C2 \n\t"
|
|
"add r27, r0 \n\t"
|
|
"adc %A0, r1 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"mul %B1, %B2 \n\t"
|
|
"add r27, r0 \n\t"
|
|
"adc %A0, r1 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"mul %C1, %A2 \n\t"
|
|
"add r27, r0 \n\t"
|
|
"adc %A0, r1 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"mul %B1, %A2 \n\t"
|
|
"add r27, r1 \n\t"
|
|
"adc %A0, r26 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"lsr r27 \n\t"
|
|
"adc %A0, r26 \n\t"
|
|
"adc %B0, r26 \n\t"
|
|
"clr r1 \n\t"
|
|
: "=&r"(intRes)
|
|
: "d"(longIn1), "d"(longIn2)
|
|
: "r26", "r27");
|
|
#endif
|
|
return intRes;
|
|
}
|
|
|
|
} // namespace math
|
|
} // namespace modules
|