Implement WebAssembly runtime and kinematics coverage
This commit is contained in:
257
wasm-core/src/sim_interpreter.c
Normal file
257
wasm-core/src/sim_interpreter.c
Normal file
@@ -0,0 +1,257 @@
|
||||
#include "sim_interpreter.h"
|
||||
|
||||
#include <ctype.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
static uint64_t fnv1a64(const unsigned char *bytes, size_t length) {
|
||||
uint64_t hash = UINT64_C(1469598103934665603);
|
||||
size_t index;
|
||||
for (index = 0; index < length; ++index) {
|
||||
hash ^= (uint64_t)bytes[index];
|
||||
hash *= UINT64_C(1099511628211);
|
||||
}
|
||||
return hash;
|
||||
}
|
||||
|
||||
static int finite_state(const sim_interpreter_state *state) {
|
||||
const double *values;
|
||||
size_t index;
|
||||
if (state == 0) return 0;
|
||||
values = &state->x;
|
||||
for (index = 0; index < 11; ++index) if (!isfinite(values[index])) return 0;
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int parse_number(const char **cursor, const char *end, double *output) {
|
||||
const char *start = *cursor;
|
||||
int sign = 1;
|
||||
int digits = 0;
|
||||
int exponent_sign = 1;
|
||||
int exponent = 0;
|
||||
long double value = 0.0L;
|
||||
long double scale = 1.0L;
|
||||
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
|
||||
if (**cursor == '-') sign = -1;
|
||||
++*cursor;
|
||||
}
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
value = value * 10.0L + (long double)(**cursor - '0');
|
||||
++*cursor;
|
||||
digits = 1;
|
||||
}
|
||||
if (*cursor < end && **cursor == '.') {
|
||||
++*cursor;
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
value = value * 10.0L + (long double)(**cursor - '0');
|
||||
scale *= 10.0L;
|
||||
++*cursor;
|
||||
digits = 1;
|
||||
}
|
||||
}
|
||||
if (!digits) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
||||
if (*cursor < end && (**cursor == 'e' || **cursor == 'E')) {
|
||||
++*cursor;
|
||||
if (*cursor < end && (**cursor == '+' || **cursor == '-')) {
|
||||
if (**cursor == '-') exponent_sign = -1;
|
||||
++*cursor;
|
||||
}
|
||||
if (*cursor == end || !isdigit((unsigned char)**cursor)) { *cursor = start; return SIM_INTERP_ERR_SYNTAX; }
|
||||
while (*cursor < end && isdigit((unsigned char)**cursor)) {
|
||||
if (exponent < 10000) exponent = exponent * 10 + (**cursor - '0');
|
||||
++*cursor;
|
||||
}
|
||||
}
|
||||
value = (long double)sign * value / scale;
|
||||
if (exponent) value *= powl(10.0L, (long double)(exponent_sign * exponent));
|
||||
*output = (double)value;
|
||||
return isfinite(*output) ? SIM_INTERP_OK : SIM_INTERP_ERR_NONFINITE;
|
||||
}
|
||||
|
||||
static int code_is(double value, int integer_code) {
|
||||
return isfinite(value) && fabs(value - (double)integer_code) <= 1e-12;
|
||||
}
|
||||
|
||||
static int axis_index(char letter) {
|
||||
switch (letter) {
|
||||
case 'X': return 0; case 'Y': return 1; case 'Z': return 2;
|
||||
case 'A': return 3; case 'B': return 4; case 'C': return 5;
|
||||
case 'U': return 6; case 'V': return 7; case 'W': return 8;
|
||||
default: return -1;
|
||||
}
|
||||
}
|
||||
|
||||
static double *axis_pointer(sim_interpreter_state *state, int index) {
|
||||
return &state->x + index;
|
||||
}
|
||||
|
||||
static int apply_g(sim_interpreter_state *state, double code, int *groups) {
|
||||
if (code_is(code, 0)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 0; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 1)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = 1; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 2) || code_is(code, 3)) { if (++groups[0] > 1) return SIM_INTERP_ERR_STATE; state->motion_mode = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 17) || code_is(code, 18) || code_is(code, 19)) { if (++groups[1] > 1) return SIM_INTERP_ERR_STATE; state->plane = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 20) || code_is(code, 21)) { if (++groups[2] > 1) return SIM_INTERP_ERR_STATE; state->units = (int)code; return SIM_INTERP_OK; }
|
||||
if (code_is(code, 90) || code_is(code, 91)) { if (++groups[3] > 1) return SIM_INTERP_ERR_STATE; state->distance_mode = (int)code; return SIM_INTERP_OK; }
|
||||
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
}
|
||||
|
||||
static int apply_m(sim_interpreter_state *state, double code, int *groups) {
|
||||
if (code_is(code, 3) || code_is(code, 4) || code_is(code, 5)) {
|
||||
if (++groups[4] > 1) return SIM_INTERP_ERR_STATE;
|
||||
state->spindle_mode = (int)code;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
if (code_is(code, 2) || code_is(code, 30)) {
|
||||
if (++groups[5] > 1) return SIM_INTERP_ERR_STATE;
|
||||
state->program_end = 1;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
return SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
}
|
||||
|
||||
static uint64_t state_hash(const sim_interpreter_state *state) {
|
||||
unsigned char bytes[sizeof(double) * 11 + sizeof(uint32_t) + sizeof(int) * 7];
|
||||
size_t offset = 0;
|
||||
const double *values = &state->x;
|
||||
size_t index;
|
||||
for (index = 0; index < 11; ++index) { memcpy(bytes + offset, values + index, sizeof(double)); offset += sizeof(double); }
|
||||
memcpy(bytes + offset, &state->line, sizeof(uint32_t)); offset += sizeof(uint32_t);
|
||||
memcpy(bytes + offset, &state->units, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->distance_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->plane, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->motion_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->spindle_mode, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->program_end, sizeof(int)); offset += sizeof(int);
|
||||
memcpy(bytes + offset, &state->status, sizeof(int)); offset += sizeof(int);
|
||||
return fnv1a64(bytes, offset);
|
||||
}
|
||||
|
||||
static int make_canonical(sim_interpreter *interpreter) {
|
||||
int written;
|
||||
const sim_interpreter_state *s = &interpreter->state;
|
||||
written = snprintf(interpreter->canonical, sizeof(interpreter->canonical),
|
||||
"{\"a\":%.17g,\"b\":%.17g,\"c\":%.17g,\"distance_mode\":%d,\"feed\":%.17g,\"line\":%u,\"motion_mode\":%d,\"plane\":%d,\"program_end\":%d,\"speed\":%.17g,\"spindle_mode\":%d,\"status\":%d,\"units\":%d,\"u\":%.17g,\"v\":%.17g,\"w\":%.17g,\"x\":%.17g,\"y\":%.17g,\"z\":%.17g}",
|
||||
s->a, s->b, s->c, s->distance_mode, s->feed, s->line, s->motion_mode, s->plane, s->program_end,
|
||||
s->speed, s->spindle_mode, s->status, s->units, s->u, s->v, s->w, s->x, s->y, s->z);
|
||||
if (written < 0 || (size_t)written >= sizeof(interpreter->canonical)) return SIM_INTERP_ERR_BUFFER;
|
||||
interpreter->canonical_length = (uint32_t)written;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
int sim_interpreter_reset(sim_interpreter *interpreter) {
|
||||
if (interpreter == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
memset(interpreter, 0, sizeof(*interpreter));
|
||||
interpreter->state.units = SIM_INTERP_UNITS_MM;
|
||||
interpreter->state.distance_mode = SIM_INTERP_ABSOLUTE;
|
||||
interpreter->state.plane = 17;
|
||||
interpreter->state.motion_mode = 1;
|
||||
interpreter->state.spindle_mode = 5;
|
||||
interpreter->state.status = SIM_INTERP_OK;
|
||||
return make_canonical(interpreter);
|
||||
}
|
||||
|
||||
int sim_interpreter_init(sim_interpreter *interpreter) {
|
||||
return sim_interpreter_reset(interpreter);
|
||||
}
|
||||
|
||||
int sim_interpreter_execute(sim_interpreter *interpreter, const char *block, uint32_t length) {
|
||||
sim_interpreter_state next;
|
||||
const char *cursor;
|
||||
const char *end;
|
||||
int groups[6] = {0};
|
||||
int seen_axis[9] = {0};
|
||||
double axis_values[9] = {0};
|
||||
int seen_feed = 0;
|
||||
int seen_speed = 0;
|
||||
double feed_value = 0.0;
|
||||
double speed_value = 0.0;
|
||||
int seen_line = 0;
|
||||
int result = SIM_INTERP_OK;
|
||||
if (interpreter == 0 || block == 0 || length == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
if (!finite_state(&interpreter->state)) return SIM_INTERP_ERR_STATE;
|
||||
if (interpreter->state.program_end) return SIM_INTERP_ERR_STATE;
|
||||
next = interpreter->state;
|
||||
cursor = block;
|
||||
end = block + length;
|
||||
while (cursor < end) {
|
||||
char letter;
|
||||
double value;
|
||||
int index;
|
||||
while (cursor < end && isspace((unsigned char)*cursor)) ++cursor;
|
||||
if (cursor >= end) break;
|
||||
if (*cursor == ';') break;
|
||||
if (*cursor == '(') {
|
||||
int closed = 0;
|
||||
++cursor;
|
||||
while (cursor < end) { if (*cursor++ == ')') { closed = 1; break; } }
|
||||
if (!closed) return SIM_INTERP_ERR_SYNTAX;
|
||||
continue;
|
||||
}
|
||||
letter = (char)toupper((unsigned char)*cursor++);
|
||||
if (letter == '%') continue;
|
||||
if (letter < 'A' || letter > 'Z') return SIM_INTERP_ERR_SYNTAX;
|
||||
result = parse_number(&cursor, end, &value);
|
||||
if (result != SIM_INTERP_OK) return result;
|
||||
switch (letter) {
|
||||
case 'G': result = apply_g(&next, value, groups); break;
|
||||
case 'M': result = apply_m(&next, value, groups); break;
|
||||
case 'N': if (seen_line++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0 || value > 4294967295.0 || floor(value) != value) result = SIM_INTERP_ERR_RANGE; else next.line = (uint32_t)value; break;
|
||||
case 'F': if (seen_feed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else feed_value = value; break;
|
||||
case 'S': if (seen_speed++) result = SIM_INTERP_ERR_STATE; else if (value < 0.0) result = SIM_INTERP_ERR_RANGE; else speed_value = value; break;
|
||||
default:
|
||||
index = axis_index(letter);
|
||||
if (index < 0) result = SIM_INTERP_ERR_UNKNOWN_CODE;
|
||||
else if (seen_axis[index]++) result = SIM_INTERP_ERR_STATE;
|
||||
else if (!isfinite(value)) result = SIM_INTERP_ERR_NONFINITE;
|
||||
if (result == SIM_INTERP_OK) axis_values[index] = value;
|
||||
break;
|
||||
}
|
||||
if (result != SIM_INTERP_OK) return result;
|
||||
}
|
||||
/* Modal words apply to the whole block, independent of their textual order. */
|
||||
if (seen_feed) next.feed = feed_value * (next.units == SIM_INTERP_UNITS_INCH ? 25.4 : 1.0);
|
||||
if (seen_speed) next.speed = speed_value;
|
||||
{
|
||||
size_t axis;
|
||||
for (axis = 0; axis < 9; ++axis) if (seen_axis[axis]) {
|
||||
double value = axis_values[axis];
|
||||
double *destination = axis_pointer(&next, (int)axis);
|
||||
if (next.units == SIM_INTERP_UNITS_INCH && axis < 3) value *= 25.4;
|
||||
if (next.distance_mode == SIM_INTERP_INCREMENTAL) *destination += value;
|
||||
else *destination = value;
|
||||
}
|
||||
}
|
||||
if (!finite_state(&next)) return SIM_INTERP_ERR_NONFINITE;
|
||||
next.line = interpreter->state.line + 1;
|
||||
next.status = SIM_INTERP_OK;
|
||||
interpreter->state = next;
|
||||
interpreter->block_hash = fnv1a64((const unsigned char *)block, length);
|
||||
result = make_canonical(interpreter);
|
||||
if (result != SIM_INTERP_OK) { interpreter->state.status = result; return result; }
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
int sim_interpreter_read_state(const sim_interpreter *interpreter, sim_interpreter_state *state) {
|
||||
if (interpreter == 0 || state == 0) return SIM_INTERP_ERR_ARGUMENT;
|
||||
*state = interpreter->state;
|
||||
return SIM_INTERP_OK;
|
||||
}
|
||||
|
||||
uint32_t sim_interpreter_canonical(const sim_interpreter *interpreter, char *output, uint32_t capacity) {
|
||||
if (interpreter == 0) return 0;
|
||||
if (output == 0 || capacity == 0) return interpreter->canonical_length + 1;
|
||||
if (capacity <= interpreter->canonical_length) return interpreter->canonical_length + 1;
|
||||
memcpy(output, interpreter->canonical, interpreter->canonical_length + 1);
|
||||
return interpreter->canonical_length + 1;
|
||||
}
|
||||
|
||||
uint64_t sim_interpreter_state_hash(const sim_interpreter *interpreter) {
|
||||
if (interpreter == 0) return 0;
|
||||
return state_hash(&interpreter->state);
|
||||
}
|
||||
|
||||
uint64_t sim_interpreter_block_hash(const sim_interpreter *interpreter) {
|
||||
return interpreter == 0 ? 0 : interpreter->block_hash;
|
||||
}
|
||||
Reference in New Issue
Block a user