Add support for float binary pattern matching

Add support to bs_get_float2 in ordet to enable
<<F:32/float>> = Bin.

Signed-off-by: Davide Bettio <davide@uninstall.it>

src/libAtomVM/bitstring.c

@@ -21,6 +21,8 @@
 
 #include "bitstring.h"
 
+#include <assert.h>
+
 static inline uint64_t from_le64(uint64_t value)
 {
     return ((((value) &0xFF) << 56) | (((value) &0xFF00) << 40) | (((value) &0xFF0000) << 24) | (((value) &0xFF000000) << 8) | (((value) &0xFF00000000) >> 8) | (((value) &0xFF0000000000) >> 24) | (((value) &0xFF000000000000) >> 40) | (((value) &0xFF00000000000000) >> 56));
@@ -350,3 +352,122 @@ bool bitstring_utf32_decode(const uint8_t *buf, size_t len, int32_t *c, enum Bit
         return true;
     }
 }
+
+bool bitstring_extract_f32(
+    term src_bin, size_t offset, avm_int_t n, enum BitstringFlags bs_flags, avm_float_t *dst)
+{
+    unsigned long capacity = term_binary_size(src_bin);
+    if (8 * capacity - offset < (unsigned long) n) {
+        return false;
+    }
+
+    if ((offset & 0x7) == 0) {
+        int byte_offset = offset >> 3;
+        const uint8_t *src = (const uint8_t *) term_binary_data(src_bin) + byte_offset;
+
+        union
+        {
+            uint32_t bits;
+            float fvalue;
+        } f32;
+
+        if (bs_flags & LittleEndianIntegerMask) {
+            f32.bits = READ_32LE_UNALIGNED(src);
+        } else {
+            f32.bits = READ_32_UNALIGNED(src);
+        }
+        *dst = f32.fvalue;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+static bool bin64_to_f32(uint64_t bin64, float *out)
+{
+    int sign = (int) ((bin64 >> 63) & 1);
+    int exponent = (int) ((bin64 >> 52) & 0x7FF) - 1023;
+    uint64_t mantissa = (bin64 >> 28) & 0xFFFFFF;
+
+    if (exponent == 1024) {
+        // not a number
+        return false;
+    } else if (exponent > 127) {
+        // too big
+        return false;
+    } else if (exponent < -150) {
+        // safely convert to 0
+        exponent = -127;
+        mantissa = 0;
+    } else if (exponent < -126) {
+        // denormalized
+        mantissa |= 0x1000000;
+        mantissa >>= (-126 - exponent);
+        exponent = -127;
+    }
+
+    mantissa = (mantissa + 1) >> 1;
+    if (mantissa & 0x800000) {
+        exponent += 1;
+        mantissa &= 0x7FFFFF;
+        mantissa >>= 1;
+    }
+
+    union
+    {
+        uint32_t u32;
+        float f32;
+    } temp;
+
+    temp.u32 = mantissa;
+    temp.u32 |= (uint32_t) (exponent + 127) << 23;
+    temp.u32 |= (uint32_t) sign << 31;
+
+    *out = temp.f32;
+
+    return false;
+}
+
+bool bitstring_extract_f64(
+    term src_bin, size_t offset, avm_int_t n, enum BitstringFlags bs_flags, avm_float_t *dst)
+{
+    unsigned long capacity = term_binary_size(src_bin);
+    if (8 * capacity - offset < (unsigned long) n) {
+        return false;
+    }
+
+    if ((offset & 0x7) == 0) {
+        int byte_offset = offset >> 3;
+        const uint8_t *src = (const uint8_t *) term_binary_data(src_bin) + byte_offset;
+
+        uint64_t bin64;
+        if (bs_flags & LittleEndianIntegerMask) {
+            bin64 = READ_64LE_UNALIGNED(src);
+        } else {
+            bin64 = READ_64_UNALIGNED(src);
+        }
+
+        _Static_assert(
+            (sizeof(avm_float_t) == 4) || (sizeof(avm_float_t) == 8), "Unsupported double size");
+
+        if (sizeof(avm_float_t) == 8) {
+            union
+            {
+                uint64_t bits;
+                double fvalue;
+            } f64;
+
+            f64.bits = bin64;
+            *dst = f64.fvalue;
+        } else if (sizeof(avm_float_t) == 4) {
+            float f32;
+            bool res = bin64_to_f32(bin64, &f32);
+            *dst = f32;
+            return res;
+        }
+        return true;
+
+    } else {
+        return false;
+    }
+}

src/libAtomVM/bitstring.h

@@ -513,6 +513,11 @@ static inline bool bitstring_match_utf32(term src_bin, size_t offset, int32_t *c
     return bitstring_utf32_decode(src, term_binary_size(src_bin) - byte_offset, c, bs_flags);
 }
 
+bool bitstring_extract_f32(
+    term src_bin, size_t offset, avm_int_t n, enum BitstringFlags bs_flags, avm_float_t *dst);
+bool bitstring_extract_f64(
+    term src_bin, size_t offset, avm_int_t n, enum BitstringFlags bs_flags, avm_float_t *dst);
+
 #ifdef __cplusplus
 }
 #endif

src/libAtomVM/opcodes.h

@@ -110,6 +110,7 @@
 #define OP_IS_FUNCTION2 115
 #define OP_BS_START_MATCH2 116
 #define OP_BS_GET_INTEGER2 117
+#define OP_BS_GET_FLOAT2 118
 #define OP_BS_GET_BINARY2 119
 #define OP_BS_SKIP_BITS2 120
 #define OP_BS_TEST_TAIL2 121

src/libAtomVM/opcodesswitch.h

@@ -5152,6 +5152,79 @@ HOT_FUNC int scheduler_entry_point(GlobalContext *glb)
                 break;
             }
 
+            case OP_BS_GET_FLOAT2: {
+                int next_off = 1;
+                uint32_t fail;
+                DECODE_LABEL(fail, code, i, next_off)
+                term src;
+                DECODE_COMPACT_TERM(src, code, i, next_off);
+                term arg2;
+                DECODE_COMPACT_TERM(arg2, code, i, next_off);
+                term size;
+                DECODE_COMPACT_TERM(size, code, i, next_off);
+                uint32_t unit;
+                DECODE_LITERAL(unit, code, i, next_off);
+                uint32_t flags_value;
+                DECODE_LITERAL(flags_value, code, i, next_off)
+
+                #ifdef IMPL_CODE_LOADER
+                    TRACE("bs_get_float2/7\n");
+                #endif
+
+                #ifdef IMPL_EXECUTE_LOOP
+                    VERIFY_IS_MATCH_STATE(src, "bs_get_float");
+                    VERIFY_IS_INTEGER(size,     "bs_get_float");
+
+                    avm_int_t size_val = term_to_int(size);
+
+                    TRACE("bs_get_float2/7, fail=%u src=%p size=%u unit=%u flags=%x\n", (unsigned) fail, (void *) src, (unsigned) size_val, (unsigned) unit, (int) flags_value);
+
+                    avm_int_t increment = size_val * unit;
+                    avm_float_t value;
+                    term bs_bin = term_get_match_state_binary(src);
+                    avm_int_t bs_offset = term_get_match_state_offset(src);
+                    bool status;
+                    switch (size_val) {
+                        case 32:
+                            status = bitstring_extract_f32(bs_bin, bs_offset, increment, flags_value, &value);
+                            break;
+                        case 64:
+                            status = bitstring_extract_f64(bs_bin, bs_offset, increment, flags_value, &value);
+                            break;
+                        default:
+                            TRACE("bs_get_float2: error extracting float.\n");
+                            status = false;
+                            JUMP_TO_ADDRESS(mod->labels[fail]);
+                            break;
+                    }
+                    if (UNLIKELY(!status)) {
+                        TRACE("bs_get_float2: error extracting float.\n");
+                        JUMP_TO_ADDRESS(mod->labels[fail]);
+                    } else {
+                        term_set_match_state_offset(src, bs_offset + increment);
+
+                        if (UNLIKELY(memory_ensure_free_opt(ctx, FLOAT_SIZE, MEMORY_NO_GC) != MEMORY_GC_OK)) {
+                            ctx->x[0] = ERROR_ATOM;
+                            ctx->x[1] = OUT_OF_MEMORY_ATOM;
+                            return term_invalid_term();
+                        }
+                        term t = term_from_float(value, &ctx->heap);
+                #endif
+
+                dreg_t dreg;
+                DECODE_DEST_REGISTER(dreg, code, i, next_off);
+
+                #ifdef IMPL_EXECUTE_LOOP
+                        WRITE_REGISTER(dreg, t);
+                        NEXT_INSTRUCTION(next_off);
+                    }
+                #endif
+                #ifdef IMPL_CODE_LOADER
+                    NEXT_INSTRUCTION(next_off);
+                #endif
+                break;
+            }
+
             case OP_BS_GET_BINARY2: {
                 int next_off = 1;
                 uint32_t fail;