zongors-reality-engine/docs/dis-like-mem2mem/udis_vm_q16-16.c

#include <errno.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <unistd.h>

// Q16.16 fixed-point macros
#define Q16_16_SCALE 16
#define Q16_16_FRACTION_MASK 0x0000FFFF
#define TO_Q16_16(x)                                                           \
  ((int32_t)((x) << Q16_16_SCALE)) // Convert integer to Q16.16
#define FROM_Q16_16(x)                                                         \
  ((int32_t)((x) >> Q16_16_SCALE)) // Convert Q16.16 to integer

#define MEMORY_SIZE 65536
int32_t memory[MEMORY_SIZE]; // Changed to signed for fixed-point support

typedef enum { OP_ADD, OP_MOV, OP_JMP, OP_HALT } Opcode;


// Convert string to Q16.16 fixed-point
int32_t string_to_q16_16(const char *str) {
    int32_t result = 0;
    int sign = 1;

    // Handle sign
    if (*str == '-') {
        sign = -1;
        str++;
    }

    // Parse integer part
    int32_t integer_part = 0;
    while (*str != '.' && *str != '\0') {
        if (*str < '0' || *str > '9') {
            return 0;  // Invalid character
        }
        integer_part = integer_part * 10 + (*str - '0');
        str++;
    }

    // Clamp integer part to 16-bit range
    if (integer_part > 32767) {
        integer_part = 32767;  // Overflow clamp
    }

    // Parse fractional part
    uint32_t frac_digits = 0;
    int frac_count = 0;

    if (*str == '.') {
        str++;  // Skip decimal point
        while (*str != '\0' && frac_count < 6) {
            if (*str < '0' || *str > '9') {
                break;
            }
            frac_digits = frac_digits * 10 + (*str - '0');
            frac_count++;
            str++;
        }
    }

    // Scale fractional part to Q16.16
    // frac_q16 = (frac_digits * 65536 + 500000) / 1000000
    // This avoids floating-point by using integer arithmetic
    uint32_t scaled_frac = 0;
    if (frac_count > 0) {
        // Pad with zeros if less than 6 digits
        while (frac_count < 6) {
            frac_digits *= 10;
            frac_count++;
        }
        // Compute scaled fractional part with rounding
        scaled_frac = (frac_digits * 65536 + 500000) / 1000000;
        if (scaled_frac > 0xFFFF) {
            scaled_frac = 0xFFFF;  // Clamp to 16-bit range
        }
    }

    // Combine integer and fractional parts
    result = (integer_part << Q16_16_SCALE) | (scaled_frac & Q16_16_FRACTION_MASK);
    return result * sign;
}

// Convert Q16.16 to string using integer-only arithmetic
int q16_16_to_string(int32_t value, char *buffer, size_t size) {
  if (size < 13) { // Minimum buffer size for "-32768.000000\0"
    if (size > 0)
      buffer[0] = '\0';
    return -1; // Buffer too small
  }

  char *buf = buffer;
  size_t remaining = size;

  // Handle sign
  if (value < 0) {
    *buf++ = '-';
    value = -value;
    remaining--;
  }

  // Extract integer and fractional parts
  int32_t integer_part = value >> Q16_16_SCALE;
  uint32_t frac = value & Q16_16_FRACTION_MASK;

  // Convert integer part to string
  char int_buf[11]; // Max 10 digits for 32-bit int
  char *int_end = int_buf + sizeof(int_buf);
  char *int_ptr = int_end;

  // Special case for zero
  if (integer_part == 0) {
    *--int_ptr = '0';
  } else {
    while (integer_part > 0 && int_ptr > int_buf) {
      *--int_ptr = '0' + (integer_part % 10);
      integer_part /= 10;
    }
  }

  // Copy integer part to output buffer
  while (int_ptr < int_end && remaining > 1) {
    *buf++ = *int_ptr++;
    remaining--;
  }

  // Add decimal point
  if (remaining > 7) { // Need space for .000000
    *buf++ = '.';
    remaining--;
  } else {
    if (remaining > 0)
      *buf = '\0';
    return size - remaining; // Truncate if insufficient space
  }

  // Convert fractional part to 6 digits
  for (int i = 0; i < 6 && remaining > 1; i++) {
    frac *= 10;
    uint32_t digit = frac >> 16;
    frac &= 0xFFFF;
    *buf++ = '0' + digit;
    remaining--;
  }

  // Null-terminate
  *buf = '\0';
  return buf - buffer; // Return length written
}

void run_vm() {
  uint32_t pc = 0;
  while (1) {
    Opcode opcode = memory[pc];
    uint32_t src1_addr = memory[pc + 1];
    uint32_t src2_addr = memory[pc + 2];
    uint32_t dest_addr = memory[pc + 3];
    pc += 4;

    if (src1_addr >= MEMORY_SIZE || src2_addr >= MEMORY_SIZE ||
        dest_addr >= MEMORY_SIZE) {
      printf("Invalid memory address!\n");
      exit(1);
    }

    switch (opcode) {
    case OP_ADD:
      // Q16.16 addition requires no scaling adjustment
      memory[dest_addr] = memory[src1_addr] + memory[src2_addr];
      break;
    case OP_MOV:
      // Direct copy preserves fixed-point representation
      memory[dest_addr] = memory[src1_addr];
      break;
    case OP_JMP:
      pc = src1_addr;
      break;
    case OP_HALT:
      return;
    default:
      printf("Unknown opcode: %d\n", opcode);
      exit(1);
    }
  }
}

int main() {
  // Initialize memory with Q16.16 values
  memory[0] = OP_ADD;
  memory[1] = 100;
  memory[2] = 101;
  memory[3] = 102;
  const char *input = "-5.0";
  memory[100] = string_to_q16_16(input); // Convert "-5.0" to Q16.16
  const char *input2 = "10.0";
  memory[101] = string_to_q16_16(input2); // Convert "10.0" to Q16.16
  memory[4] = OP_HALT;

  run_vm();

  char buffer[13]; // Sufficient for Q16.16 format

  // Convert result back to integer
  q16_16_to_string(memory[102], buffer, sizeof(buffer));
  printf("Result at address 102: %s\n", buffer); // Output: 5.0
  return 0;
}