Firmware/hpsg5-controller_v2-stm32g4
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7ff62ccabcc8789bbfeffd73e09f5c5ff4854d56
hpsg5-controller_v2-stm32g4/Core/Phi/phi.c

1343 lines
61 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/**
* @file phi.c
* @brief T06215 compact-port — monolithic single-translation-unit
* implementation of the injection-angle producer chain.
*
* All seven producer stages and the shared submap helpers are inlined
* directly into this file as `static` functions (1:1 translations of
* the verbose modules under variants/T06215/src/, brought along
* verbatim so per-block disassembly-address comments remain
* authoritative). The producer itself (s_final_injection_angle @
* 0x754D) is byte-equivalent to T06211's FUN_7453 — see
* variants/T06215/README.md — so the inlined bodies and assembly
* address ranges are shared with T06211. The call order in
* `phi_service` matches T06211's fused-scheduler walk
* (variants/T06211/docs/open-questions.md §4 / §5).
*
* Pipeline (Stages 2–7 in `phi_service`):
* 2. compute_tein_overtemp_guard — port of FUN_5ca1 (RW7A seed addend)
* 3. compute_accel_comp_gain — port of FUN_72b0 (3-submap pipeline → *(0x0164))
* 3b. compute_angle_accumulator_3d — port of FUN_722e (3-D trilinear → RW52)
* 4. compute_accel_comp_offset — port of FUN_732d (Δ-rpm × gain → RW3C, sets REC.0)
* 5. compute_gate_0220 — port of orphan @ 0x77ff (RPM hysteresis)
* 6. compute_tein_valve_fault_guard — port of FUN_62a2 (Phase-1/2 byte state machine → RW7A)
* 7. compute_target_injection_angle — port of FUN_7453 (sole writer of RW48 / RW5A)
*
* Every MCS-96 arithmetic choice (MUL vs MULU, SHRA vs SHR logical, JGE
* vs JC) mirrors the disassembly — see the per-block comments with
* disassembly address ranges, and variants/T06215/src/ for the
* long-form function-level commentary these stages were ported from.
*/
#include "phi.h"
#include <string.h>
/* ══════════════════════════════════════════════════════════════════════
* Internal helpers — submap infrastructure
*
* 1:1 translations of the four shared helpers that the producer stages
* below call (eval_submap_to_scratch, combine_two_submaps_to_word,
* combine_three_submaps_to_word, refine_submap_result), plus two
* file-static utilities (bytes_to_words, bilinear_at_plane).
*
* Brought verbatim from variants/T06215/src/submap_eval.c with
* external linkage demoted to `static`.
* ══════════════════════════════════════════════════════════════════════ */
/*
* 1:1 translation of eval_submap_to_scratch @ 0x81db–0x8236.
*
* Populates scratch with {stride_bytes, delta, fraction, index_bytes}
* so downstream interpolators can locate the input within the
* (descending-ordered) axis and linearly interpolate between cells.
*/
static void eval_submap_to_scratch(const submap_descriptor_t *desc,
submap_scratch_t *scratch)
{
/* 0x81e3–0x81e7: read the input variable via the descriptor's pointer */
int16_t input_val = *desc->input_var;
/* 0x81ea–0x81f1: stride_bytes = stride_items << 1 */
int16_t stride_bytes = (int16_t)((uint16_t)desc->stride_items << 1);
scratch->stride_bytes = stride_bytes;
const int16_t *axis = desc->axis;
/* 0x81fc–0x81ff: compare with axis[0]; if input >= axis[0] take the
* edge case that places the interpolator at axis[0]. */
if (input_val >= axis[0]) {
/* 0x8226–0x8232: delta=2, fraction=2, index_bytes=2 */
scratch->delta = 2;
scratch->fraction = 2;
scratch->index_bytes = 2;
return;
}
/* 0x8201–0x8204: descending scan. Loop while input < axis[i].
* Exit condition: input >= axis[i]; i.e. input falls in
* [axis[i], axis[i-1]). We already know input < axis[0]. */
int16_t i = 1;
while (input_val < axis[i]) {
i++;
/* Calibration invariant: axis[stride_items-1] == 0 and input >= 0,
* so the loop terminates before running off the end. */
}
/* 0x820a–0x820e: delta = axis[i-1] − axis[i] (positive on descending) */
scratch->delta = (int16_t)(axis[i - 1] - axis[i]);
/* 0x8215–0x8218: fraction = input − axis[i] (0 ≤ fraction ≤ delta) */
scratch->fraction = (int16_t)(input_val - axis[i]);
/* 0x821c–0x8221: index_bytes = byte offset from axis[0] = i × 2 */
scratch->index_bytes = (int16_t)(i * 2);
}
/*
* Byte-offset helper: the MCS-96 treats scratch index_bytes and
* stride_bytes as byte offsets but the C pointer arithmetic here
* is on int16_t (word) arrays. All byte offsets are guaranteed
* even by the producer, so dividing by 2 is exact.
*/
static inline int32_t bytes_to_words(int16_t byte_offset)
{
/* signed arithmetic: SHRAL in the assembly is sign-preserving */
return (int32_t)byte_offset / 2;
}
/*
* 1:1 translation of combine_two_submaps_to_word @ 0x8258–0x82b4.
*
* 2-D bilinear interpolation. scratch_x = inner axis (per-row),
* scratch_y = outer axis (between-row step = stride_x_bytes).
*/
static int16_t combine_two_submaps_to_word(const int16_t *data_table,
const submap_scratch_t *scratch_x,
const submap_scratch_t *scratch_y)
{
/* 0x8264–0x826f: compute byte offset to (X, Y_lo) cell.
* y_off_bytes = (idx_y × stride_x_bytes) / 2 (signed MUL + SHRAL)
* total_off = y_off_bytes + idx_x_bytes
*/
int32_t y_prod = (int32_t)scratch_y->index_bytes * (int32_t)scratch_x->stride_bytes;
int16_t y_off_bytes = (int16_t)(y_prod >> 1);
int16_t total_off_ylo = (int16_t)(y_off_bytes + scratch_x->index_bytes);
int32_t word_idx_ylo = bytes_to_words(total_off_ylo);
/* 0x8276–0x828a: lerp X within Y_lo row.
* upper = data[idx-1] (descending → prev word is higher axis value)
* lower = data[idx]
* lerp = lower + (upper − lower) × frac_x / delta_x
*/
int16_t lower_ylo = data_table[word_idx_ylo];
int16_t upper_ylo = data_table[word_idx_ylo - 1];
int32_t dx_ylo = (int32_t)(int16_t)(upper_ylo - lower_ylo) * (int32_t)scratch_x->fraction;
int16_t lerp_ylo = (int16_t)((int16_t)(dx_ylo / scratch_x->delta) + lower_ylo);
/* 0x828d: step to Y_hi (one row back, i.e. −stride_x_bytes) */
int32_t word_idx_yhi = word_idx_ylo - bytes_to_words(scratch_x->stride_bytes);
/* 0x8290–0x82a1: lerp X within Y_hi row */
int16_t lower_yhi = data_table[word_idx_yhi];
int16_t upper_yhi = data_table[word_idx_yhi - 1];
int32_t dx_yhi = (int32_t)(int16_t)(upper_yhi - lower_yhi) * (int32_t)scratch_x->fraction;
int16_t lerp_yhi = (int16_t)((int16_t)(dx_yhi / scratch_x->delta) + lower_yhi);
/* 0x82a4–0x82b1: lerp Y between lerp_yhi and lerp_ylo */
int32_t dy = (int32_t)(int16_t)(lerp_yhi - lerp_ylo) * (int32_t)scratch_y->fraction;
return (int16_t)((int16_t)(dy / scratch_y->delta) + lerp_ylo);
}
/*
* Helper: 2-D bilinear at a fixed Z plane. Factored so the 3-D version
* below reads clearly. base_word_idx points to (X_lo, Y_lo) within the
* chosen Z plane.
*/
static int16_t bilinear_at_plane(const int16_t *data_table,
int32_t base_word_idx,
const submap_scratch_t *sx,
const submap_scratch_t *sy)
{
int16_t lower_ylo = data_table[base_word_idx];
int16_t upper_ylo = data_table[base_word_idx - 1];
int32_t dx_ylo = (int32_t)(int16_t)(upper_ylo - lower_ylo) * (int32_t)sx->fraction;
int16_t lerp_ylo = (int16_t)((int16_t)(dx_ylo / sx->delta) + lower_ylo);
int32_t yhi = base_word_idx - bytes_to_words(sx->stride_bytes);
int16_t lower_yhi = data_table[yhi];
int16_t upper_yhi = data_table[yhi - 1];
int32_t dx_yhi = (int32_t)(int16_t)(upper_yhi - lower_yhi) * (int32_t)sx->fraction;
int16_t lerp_yhi = (int16_t)((int16_t)(dx_yhi / sx->delta) + lower_yhi);
int32_t dy = (int32_t)(int16_t)(lerp_yhi - lerp_ylo) * (int32_t)sy->fraction;
return (int16_t)((int16_t)(dy / sy->delta) + lerp_ylo);
}
/*
* 1:1 translation of FUN_82b5 @ 0x82b5–0x838a — 3-D trilinear combine.
* Inner = A (X), middle = B (Y), outer = C (Z).
*/
static int16_t combine_three_submaps_to_word(const int16_t *data_table,
const submap_scratch_t *scratch_a,
const submap_scratch_t *scratch_b,
const submap_scratch_t *scratch_c)
{
/* 0x82c5–0x82cc: stride_per_Cplane (bytes) = stride_a × stride_b / 2 */
int32_t prod_ab = (int32_t)scratch_a->stride_bytes * (int32_t)scratch_b->stride_bytes;
int16_t stride_cplane_bytes = (int16_t)(prod_ab >> 1);
/* 0x82d1–0x82d6: Z byte offset = stride_cplane × idx_c / 2 */
int32_t prod_z = (int32_t)stride_cplane_bytes * (int32_t)scratch_c->index_bytes;
int16_t z_off_bytes = (int16_t)(prod_z >> 1);
/* 0x82d9–0x82e1: Y byte offset = idx_b × stride_a / 2 */
int32_t prod_y = (int32_t)scratch_b->index_bytes * (int32_t)scratch_a->stride_bytes;
int16_t y_off_bytes = (int16_t)(prod_y >> 1);
/* 0x82e4–0x82eb: base offset for (X, Y, Z_low) */
int16_t base_off_bytes = (int16_t)(y_off_bytes + scratch_a->index_bytes + z_off_bytes);
int32_t base_idx_clo = bytes_to_words(base_off_bytes);
/* 0x82f0–0x832e: 2-D bilinear at C_low plane */
int16_t v_clo = bilinear_at_plane(data_table, base_idx_clo, scratch_a, scratch_b);
/* 0x8335: step back one C plane → C_hi */
int32_t base_idx_chi = base_idx_clo - bytes_to_words(stride_cplane_bytes);
/* 0x833a–0x8375: 2-D bilinear at C_hi plane */
int16_t v_chi = bilinear_at_plane(data_table, base_idx_chi, scratch_a, scratch_b);
/* 0x8378–0x8387: lerp C between v_chi and v_clo */
int32_t dz = (int32_t)(int16_t)(v_chi - v_clo) * (int32_t)scratch_c->fraction;
return (int16_t)((int16_t)(dz / scratch_c->delta) + v_clo);
}
/*
* 1:1 translation of refine_submap_result @ 0x8237–0x8257.
*
* The ROM walks the data table in BYTES via 0x6[scratch] (index_bytes),
* then takes -0x2[ptr] (the previous int16) as `upper` and *ptr as
* `lower`. In C, that's data_table[index_words] = lower and
* data_table[index_words - 1] = upper, where
* index_words = index_bytes / 2.
*/
static int16_t refine_submap_result(const int16_t *data_table,
const submap_scratch_t *scratch)
{
int32_t index_words = bytes_to_words(scratch->index_bytes);
int16_t lower = data_table[index_words];
int16_t upper = data_table[index_words - 1];
int32_t prod = (int32_t)(int16_t)(upper - lower) * (int32_t)scratch->fraction;
int16_t quot = (int16_t)(prod / scratch->delta);
return (int16_t)(quot + lower);
}
/* ══════════════════════════════════════════════════════════════════════
* Producer stages — 1:1 ports of the seven verbose modules under
* variants/T06215/src/, brought along verbatim with external linkage
* demoted to `static`. Each docstring identifies the originating ROM
* function (FUN_xxxx @ 0xyyyy).
* ══════════════════════════════════════════════════════════════════════ */
/*
* Translation of FUN_5ca1 @ 0x5ca1–0x5d57 (T06211 variant), scoped to the
* tein_overtemp_guard producer.
*
* The ROM's prologue (0x5ca7–0x5cf6) is a sensor-raw temperature scaler:
* it reads *(0x0144), scales via MULU+ADD, range-validates against
* rom_6014/rom_6016, and either commits the result to *(0x0146)
* (temperature) or reloads from temperature on out-of-range. This C port
* treats `temperature` as the input boundary, so that scaler is omitted —
* `rt->temperature` is supplied externally and consumed directly.
*
* Peripheral status writes (RFE bits 0/7, RWEA bits 5/6) at 0x5cfe /
* 0x5d01 / 0x5d4a / 0x5d4d are NOT modelled — sensor-fault status bits
* to the diagnostics subsystem, no feedback into the angle pipeline.
*/
static void compute_tein_overtemp_guard(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x5ca1–0x5ca6: prologue (PUSH RW1C, RW1E, RW20) — handled by C locals */
/* 0x5ca7–0x5cf6 (sensor-raw temperature scaler) is scoped out — see
* function docstring. We pick up at 0x5cf7 with rw1e seeded directly
* from rt->temperature. */
int16_t rw1c;
int16_t rw1e = rt->temperature;
/* 0x5cf7–0x5cfd: SUB RW1E, CAL[RWA4]+0x92 ; JNH LAB_5d45 (UNSIGNED)
* If the subtraction underflows (temperature <= cal_92 unsigned), go
* to the reset path — tein_overtemp_guard is forced to 0. */
if ((uint16_t)rw1e <= (uint16_t)cal->cal_92) {
goto reset_path;
}
rw1e = (int16_t)((uint16_t)rw1e - (uint16_t)cal->cal_92);
/* 0x5cfe–0x5d04: RFE |= 0x80, RWEA |= 0x20 (peripheral; not modelled) */
/* 0x5d05–0x5d0a: MULU RL1C, RW1E, CAL[RWA4]+0x94 (3-op UNSIGNED 16×16→32)
* RL1C = rw1e × cal_94. Low word lands in rw1c, high word in rw1e. */
{
uint32_t rl1c = (uint32_t)(uint16_t)rw1e * (uint32_t)(uint16_t)cal->cal_94;
rw1c = (int16_t)(uint16_t)(rl1c & 0xFFFFu);
rw1e = (int16_t)(uint16_t)(rl1c >> 16);
}
/* 0x5d0b–0x5d16: CMP RW1C, CAL[RWA4]+0x96 ; JNH LAB_5d17 ; LD RW1C, cal_96
* UNSIGNED upper-clamp. JNH = unsigned <=, so the JNH skips the
* clamp when rw1c <= cal_96; we clamp when rw1c > cal_96. */
if ((uint16_t)rw1c > (uint16_t)cal->cal_96) {
rw1c = cal->cal_96;
}
/* 0x5d17–0x5d19: LD RW20, RW40 — copy rpm into RW20 */
uint16_t rw20 = rt->rpm;
/* 0x5d1a–0x5d20: CMP RW20, CAL[RWA4]+0x98 ; JNH LAB_5d45 (UNSIGNED)
* If rpm <= cal_98 → reset (tein_overtemp_guard = 0). */
if (rw20 <= (uint16_t)cal->cal_98) {
goto reset_path;
}
/* 0x5d21–0x5d27: CMP RW20, CAL[RWA4]+0x9a ; JH LAB_5d3e (UNSIGNED)
* If rpm > cal_9a → use raw rw1c (no rpm scaling). */
if (rw20 > (uint16_t)cal->cal_9a) {
goto store_011a;
}
/* 0x5d28–0x5d3d: rpm-blend
* RW1E = rpm − cal_98 (3-op SUB; fresh write, no carryover)
* RL1C = rw1c × rw1e (2-op MULU)
* RW20 = cal_9a − cal_98 (3-op SUB) — actually 2-op rewrite
* RL1C /= rw20 (DIVU)
* Result: rw1c (quotient) is the rpm-scaled tein_overtemp_guard value. */
{
uint16_t numer_b = (uint16_t)(rw20 - (uint16_t)cal->cal_98);
uint32_t rl1c = (uint32_t)(uint16_t)rw1c * (uint32_t)numer_b;
uint16_t denom = (uint16_t)((uint16_t)cal->cal_9a - (uint16_t)cal->cal_98);
if (denom == 0u) {
/* MCS-96 DIVU traps on zero divisor; defensively guard.
* Real ROM relies on cal_9a > cal_98 being a cal-block
* invariant (the unsigned tests above guarantee we only
* reach here when cal_98 < rpm <= cal_9a, so cal_9a > 0;
* but cal_9a == cal_98 with rpm strictly between them is
* impossible — yet the guard costs nothing). */
goto reset_path;
}
rw1c = (int16_t)(uint16_t)(rl1c / denom);
/* remainder in rw1e is unused downstream */
}
store_011a:
/* 0x5d3e–0x5d43: ST RW1C, *(0x011a) ; SJMP LAB_5d51 */
rt->tein_overtemp_guard = rw1c;
return;
reset_path:
/* 0x5d45–0x5d50: ST ZR, *(0x011a)
* ANDB RFE, #0x7F (peripheral clear — not modelled)
* AND RWEA, #0xFFDF (peripheral clear — not modelled) */
rt->tein_overtemp_guard = 0;
/* 0x5d51–0x5d57: epilogue — handled by C */
}
/*
* 1:1 translation of FUN_72b0 @ 0x72b0–0x732c (T06211 variant).
*
* Per-tick producer of `rt->accel_comp_gain` (*(0x0164)). Pipeline:
* 1. eval RPM submap (CAL+0x58)
* 2. eval inj_qty_demand submap (CAL+0x60)
* 3. combine_two_submaps_to_word over the (RPM × demand) table at CAL+0x70
* 4. eval temperature submap (CAL+0x68)
* 5. refine_submap_result over the 1-D table at CAL+0x72
* 6. signed MUL: rl1c = refined × combined; SHLL by 8; high word → rt->accel_comp_gain
*/
/* Forward declaration — compute_angle_accumulator_3d (defined below) embeds
* the Alternate_phiad_calc gate that conditionally calls this producer.
* The producer body sits further down in the file alongside its gated
* wrapper, so the forward decl lets the gate compile against the prototype. */
static void compute_accel_comp_offset(runtime_state_t *rt, const calibration_t *cal);
static void compute_accel_comp_gain(runtime_state_t *rt, const calibration_t *cal)
{
submap_scratch_t scratch_rpm;
submap_scratch_t scratch_demand;
submap_scratch_t scratch_temp;
/* 0x72b0–0x72c3: eval RPM submap. */
eval_submap_to_scratch(&cal->desc_accel_rpm, &scratch_rpm);
/* 0x72c7–0x72da: eval inj_qty_demand submap. */
eval_submap_to_scratch(&cal->desc_accel_demand, &scratch_demand);
/* 0x72de–0x72eb: 2-D combine over the (RPM × demand) table at CAL+0x70. */
int16_t combined = combine_two_submaps_to_word(cal->accel_combine_table,
&scratch_rpm,
&scratch_demand);
/* 0x72ef: ADDB R1D, *(0x0408). *(0x0408) is unwritten in this ROM
* (no textual writers found by grep; defaults to boot zero). The
* add is therefore a no-op in T06211's runtime. Same caveat as
* T06031 — flag in open-questions if a future pump family populates
* 0x0408. */
/* 0x72f4: ST RW1C, *(0x034a) — caches `combined` for the MUL at
* 0x731e. We just keep it in the C local. */
/* 0x72f9–0x730c: eval temperature submap. */
eval_submap_to_scratch(&cal->desc_accel_temp, &scratch_temp);
/* 0x7310–0x731a: 1-D refine over the 4-word temperature table at CAL+0x72. */
int16_t refined = refine_submap_result(cal->accel_refine_table, &scratch_temp);
/* 0x731e–0x7323: signed MUL — FE prefix on `MUL RL1C, *(0x034a)`. */
int32_t prod = (int32_t)refined * (int32_t)combined;
/* 0x7324–0x7326: SHLL RL1C, #0x8 — logical shift-left long by 8. */
uint32_t shifted = ((uint32_t)prod) << 8;
/* 0x7327–0x732b: ST RW1E, *(0x0164) — store the HIGH word of the
* shifted product. */
rt->accel_comp_gain = (int16_t)(uint16_t)(shifted >> 16);
/* 0x732c: RET. */
}
/*
* 1:1 translation of FUN_722e @ 0x722e–0x72af (T06211).
*
* Three RWC6-relative submap evals (RPM, demand, angle_dec_cmd) through
* the 3-D trilinear combine → writes rt->angle_accumulator (RW52) and
* rt->scratch_0158 debug mirror. The conditional FUN_72b0 / FUN_732d
* sub-calls inside the ROM body (0x728b–0x72af) are NOT replicated here —
* they are separate phi_service stages (Stage 3 / Stage 4).
*/
static void compute_angle_accumulator_3d(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x7241–0x7248: PUSH #0x184 / PUSH RW2A (=RWC6+0) / LCALL FUN_6fb8.
* Evaluates the RPM axis descriptor at RWC6+0 into the scratch buffer
* logically mapped to address 0x0184. */
eval_submap_to_scratch(&cal->desc_rpm, &rt->scratch_rpm);
/* 0x724d–0x7258: ADD RW2A,#8 (→RWC6+8) / PUSH #0x194 / PUSH RW2A / LCALL FUN_6fb8.
* Evaluates the inj_qty_demand axis descriptor at RWC6+8 into scratch@0x0194. */
eval_submap_to_scratch(&cal->desc_demand, &rt->scratch_demand);
/* 0x725d–0x7268: ADD RW2A,#8 (→RWC6+0x10) / PUSH #0x18c / PUSH RW2A / LCALL FUN_6fb8.
* Evaluates the angle_dec_cmd axis descriptor at RWC6+0x10 into scratch@0x018c. */
eval_submap_to_scratch(&cal->desc_dec_cmd, &rt->scratch_dec_cmd);
/* 0x726d–0x727e: PUSH #0x18c / PUSH #0x194 / PUSH #0x184
* ADD RW2A,#0xa (→RWC6+0x1a) / PUSH [RW2A] / LCALL FUN_7092.
* Axis roles: A=inner(X)=rpm, B=middle(Y)=demand, C=outer(Z)=dec_cmd. */
int16_t result = combine_three_submaps_to_word(cal->data_table_3d,
&rt->scratch_rpm,
&rt->scratch_demand,
&rt->scratch_dec_cmd);
/* 0x7283–0x7287: ST RW1C, 0x158,TABLE[ZR] — debug mirror at *(0x0158). */
rt->scratch_0158 = result;
/* 0x7288–0x728a: LD RW52, RW1C — write angle_accumulator. */
rt->angle_accumulator = result;
/* 0x728b–0x72af: Alternate_phiad_calc embedded gate (FUN_5df3 @
* 0x5e50–0x5e62 in T06215, analog of T06031's FUN_5dd0 tail).
*
* Below the hysteresis threshold (gate_0220 == 0) the live ROM both
* recomputes the accel_comp_gain and force-flags REC.1 so the next
* Try_calc_accel_offset visit will produce a value despite low RPM.
* Stage 3 of phi_service has already run compute_accel_comp_gain
* unconditionally this tick, so we only need to mirror the REC.1 flag
* here.
*
* The compute_accel_comp_offset call is gated on REC.2 (set by
* compute_accel_comp_offset_gated when phi_per_cylinder_event has
* fired this cylinder cycle) AND REC.0 clear (consumer has drained
* any prior value). This is the PTS-event fallback path — at high
* RPM the per-cylinder hook will have already set REC.0, so this
* gate skips and the producer fires from the per-cylinder side
* exclusively. */
if (rt->gate_0220 == 0u) {
rt->rec = (uint8_t)(rt->rec | 0x02u);
}
if (((rt->rec & 0x04u) != 0u) && ((rt->rec & 0x01u) == 0u)) {
compute_accel_comp_offset(rt, cal);
}
}
/* ══════════════════════════════════════════════════════════════════════
* compute_temp_phi_comp — 1:1 translation of FUN_6b2a @ 0x6b2a (sign-aware
* wrapper) + FUN_6b4e @ 0x6b4e (unsigned 32-bit IIR step).
*
* Sole producer of `rt->rw9e` — the high word of the signed 32-bit
* IIR-filter accumulator `{rt->rw9e:rt->rw9c}`. Low-passes the
* rpm-derived term `rt->rw9a` (produced by compute_temp_comp_factor at
* 0x6AF9). `rt->rw9e` is consumed by the same compute_temp_comp_factor
* at 0x6AFC.
*
* Cadence: in the ROM the IIR step runs from Timer_1khz (1 kHz) gated
* by an R94 prescaler that reloads from DAT_6061 = 0x64 = 100, so the
* step actually fires at 10 Hz. The compact port preserves both layers:
* the host calls `phi_tick_1khz` every 1 ms; the prescaler in
* `rt->temp_phi_comp_r94` gates the IIR step internally.
*
* Coefficients (per-variant calibration, Q16 unsigned fractions):
* - cal->cal_82 — input gain `b` (CAL[RWA4+0x82]).
* - cal->cal_84 — pole `a` (CAL[RWA4+0x84]).
* On this ROM cal_82 + (uint16_t)cal_84 == 0x10000, giving unity DC gain.
* ══════════════════════════════════════════════════════════════════════ */
#define TEMP_PHI_COMP_R94_RELOAD ((uint8_t)100) /* DAT_6061 = 0x64 */
/*
* 1:1 port of FUN_6b4e @ 0x6b4e.
*
* 0x6b4e: MULU RL1C, RW9C, CAL[0x84] ; RL1C = RW9C *u CAL[0x84]
* 0x6b54: ST RW1E, RW9C ; RW9C := high(RW9C *u CAL[0x84])
* 0x6b57: MULU RL1C, RW20, CAL[0x84] ; RL1C = RW20 *u CAL[0x84]
* 0x6b5d: ADD RW9C, RW1C ; RW9C += low(RW20 *u CAL[0x84]); sets C
* 0x6b60: ADDC RW1E, ZR ; RW1E += 0 + C
* 0x6b63: LD RW20, RW1E ; RW20 := high(RW20 *u CAL[0x84]) + carry
* 0x6b66: MULU RL1C, RW9A, CAL[0x82] ; RL1C = RW9A *u CAL[0x82]
* 0x6b6c: ADD RW9C, RW1C ; RW9C += low(RW9A *u CAL[0x82]); sets C
* 0x6b6f: ADDC RW20, RW1E ; RW20 += high(RW9A *u CAL[0x82]) + C
* 0x6b72: RET
*
* The trace operates on the {RW20:RW9C} pair where RW20 is the caller's
* working high-word copy (sign-flipped or not by the FUN_6b2a wrapper).
* All multiplies are unsigned (MULU); all carries propagate exactly as
* the ADD/ADDC chain in the assembly.
*/
static void iir_step_unsigned(uint16_t *rw20,
uint16_t *rw9c,
uint16_t rw9a,
uint16_t cal_82,
uint16_t cal_84)
{
uint16_t rw1c, rw1e;
uint32_t prod;
/* 0x6b4e + 0x6b54: rw9c becomes the HIGH word of (rw9c * a). */
prod = (uint32_t)*rw9c * (uint32_t)cal_84;
rw1e = (uint16_t)(prod >> 16);
*rw9c = rw1e;
/* 0x6b57: prod = rw20 * a; rw1c = low, rw1e = high. */
prod = (uint32_t)*rw20 * (uint32_t)cal_84;
rw1c = (uint16_t)(prod & 0xFFFFu);
rw1e = (uint16_t)(prod >> 16);
/* 0x6b5d / 0x6b60: ADD rw9c, rw1c — capture carry, propagate to rw1e. */
{
uint32_t s = (uint32_t)*rw9c + rw1c;
*rw9c = (uint16_t)(s & 0xFFFFu);
rw1e = (uint16_t)(rw1e + (s >> 16));
}
/* 0x6b63: rw20 := rw1e (= high(rw20*a) + carry). */
*rw20 = rw1e;
/* 0x6b66: prod = rw9a * b. */
prod = (uint32_t)rw9a * (uint32_t)cal_82;
rw1c = (uint16_t)(prod & 0xFFFFu);
rw1e = (uint16_t)(prod >> 16);
/* 0x6b6c / 0x6b6f: ADD rw9c, rw1c — propagate carry to rw20. */
{
uint32_t s = (uint32_t)*rw9c + rw1c;
*rw9c = (uint16_t)(s & 0xFFFFu);
*rw20 = (uint16_t)(*rw20 + rw1e + (uint16_t)(s >> 16));
}
}
/*
* 1:1 port of FUN_6b2a @ 0x6b2a — sign-aware wrapper around the unsigned IIR step.
*
* 0x6b30: LD RW20, RW9E
* 0x6b33: JBC R21, 0x7, 0x6b42 ; bit 7 of R21 = sign bit of RW20 (= RW9E)
* 0x6b36: NEG RW9C ; RW9C := -RW9C (two's complement)
* 0x6b38: NOT RW20 ; RW20 := ~RW9E (one's complement)
* 0x6b3a: SCALL FUN_6b4e
* 0x6b3c: NOT RW20 ; un-negate
* 0x6b3e: NEG RW9C ; un-negate
* 0x6b40: SJMP 0x6b44
* 0x6b42: SCALL FUN_6b4e ; RW9E >= 0 path: run IIR step directly
* 0x6b44: LD RW9E, RW20 ; commit new high word
*
* The pre/post (NEG, NOT) pair is two's-complement negation of the
* {RW9E:RW9C} pair when RW9C != 0, and ~RW9E (off-by-one) when RW9C == 0.
* That asymmetry is preserved verbatim — it's the ROM's actual behaviour.
*/
void phi_tick_1khz(phi_state_t *state, const phi_cal_t *cal)
{
runtime_state_t *rt = &state->rt;
/* Mirrors the Timer_1khz prescaler at 0x79e2:
* R94 = R94 - 1; if (R94 == 0) { run; R94 = DAT_6061; } */
if (--rt->temp_phi_comp_r94 != 0u) {
return;
}
rt->temp_phi_comp_r94 = TEMP_PHI_COMP_R94_RELOAD;
/* 0x6b30: RW20 = RW9E. */
uint16_t rw20 = (uint16_t)rt->rw9e;
uint16_t rw9c = (uint16_t)rt->rw9c;
uint16_t rw9a = (uint16_t)rt->rw9a;
uint16_t cal_82 = (uint16_t)cal->cal_82;
uint16_t cal_84 = (uint16_t)cal->cal_84;
if ((int16_t)rt->rw9e < 0) {
/* 0x6b36 / 0x6b38: NEG RW9C, NOT RW20. */
rw9c = (uint16_t)(-(int16_t)rw9c);
rw20 = (uint16_t)~rw20;
iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84);
/* 0x6b3c / 0x6b3e: NOT RW20, NEG RW9C. */
rw20 = (uint16_t)~rw20;
rw9c = (uint16_t)(-(int16_t)rw9c);
} else {
/* 0x6b42: direct call when RW9E sign bit clear. */
iir_step_unsigned(&rw20, &rw9c, rw9a, cal_82, cal_84);
}
/* 0x6b44: commit. */
rt->rw9e = (int16_t)rw20;
rt->rw9c = (int16_t)rw9c;
}
/*
* 1:1 translation of orphan calc_temp_comp_factor @ 0x6AE7–0x6B29
* (T06215 variant; algorithmically byte-equivalent to T06211 FUN_5DAB
* @ 0x5DAB).
*
* Per-tick producer of `rt->temp_comp_factor` (the runtime mirror of
* *(0x02FC) in T06215; T06211 uses *(0x02F4)). Reached on-tick by
* `LCALL calc_temp_comp_factor` at 0x7B14 inside the per-cylinder gate
* that precedes the orphan scheduler at FUN_7b1c.
*
* Both MULs in this function carry the FE prefix → signed 16×16→32.
* The final store at 0x6B1E is `ST RW1C, temp_comp` — the LOW word of
* the second product, NOT the high word.
*
* The R0CB-gated `ADD RW20, RWCE` branch at 0x6B00–0x6B07 is
* INTENTIONALLY DROPPED — both register-bank slots are observed zero in
* nominal operation, so the branch is dead. Restore it if a future
* live-ECU dump shows R0CB transitioning out of zero.
*
* See variants/T06211/docs/open-questions.md §9.
*/
static void compute_temp_comp_factor(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x6AE7–0x6AEC: prologue (PUSH RW1C, RW1E, RW20) — handled by C locals. */
/* 0x6AED–0x6AEF: LD RW1C, RW40 — rw1c = rpm. */
int16_t rpm_signed = (int16_t)rt->rpm;
/* 0x6AF0–0x6AF5: MUL RL1C, RW1C, temp_comp_dynamic — signed (FE prefix);
* cal_temp_comp_switch_dynamic mirrors temp_comp_dynamic.
* 0x6AF6–0x6AF8: SHRAL RL1C, #2 — arithmetic right shift on the
* 32-bit signed product. */
int32_t rl1c = (int32_t)rpm_signed
* (int32_t)cal->cal_temp_comp_switch_dynamic;
rl1c = rl1c >> 2; /* arithmetic */
int16_t rw1e = (int16_t)((rl1c >> 16) & 0xFFFF); /* high word */
/* 0x6AF9–0x6AFB: LD RW9A, RW1E — publish high word as state.
* Consumed by other orphans outside the RW48 chain. */
rt->rw9a = rw1e;
/* 0x6AFC–0x6AFF: SUB RW20, RW9E, RW1E (3-op) — rw20 = RW9E − rw1e. */
int16_t rw20 = (int16_t)(rt->rw9e - rw1e);
/* 0x6B00–0x6B07: R0CB-gated `ADD RW20, RWCE` — DROPPED (see docstring). */
/* 0x6B08–0x6B0C: ADD RW20, *(0x0146) — fold in temperature. */
rw20 = (int16_t)(rw20 + rt->temperature);
/* 0x6B0D–0x6B11: SUB RW20, [RWA4+0x007E] — temperature reference. */
rw20 = (int16_t)(rw20 - cal->cal_7e);
/* 0x6B12–0x6B16: SUB RW20, *(0x0402) — sign-extended boot byte
* (cal_byte_402 = 0xFFF2 = -14 in this T06215 image). */
rw20 = (int16_t)(rw20 - cal->cal_byte_402);
/* 0x6B17–0x6B1D: MUL RL1C, RW20, temp_comp_complete — signed (FE prefix);
* cal_temp_comp_switch_complete mirrors temp_comp_complete.
* With switch == 1, rl1c == rw20 (sign-extended). */
int32_t prod = (int32_t)rw20
* (int32_t)cal->cal_temp_comp_switch_complete;
/* 0x6B1E–0x6B22: ST RW1C, temp_comp — LOW word of RL1C.
* NOTE: stores the LOW 16 bits of the signed 32-bit product, not
* the high word. Anything that overflows int16 wraps. */
rt->temp_comp_factor = (int16_t)((uint32_t)prod & 0xFFFFu);
/* 0x6B23–0x6B29: epilogue. */
}
/*
* 1:1 translation of orphan compute_angle_kick_2d @ 0x6A94–0x6AE6
* (T06215 variant; algorithmically byte-equivalent to T06211 FUN_5D58
* @ 0x5D58).
*
* Per-tick producer of `rt->angle_kick_2d` (RW3E). Reached by
* `LCALL compute_angle_kick_2d` at 0x7B76 in the orphan scheduler
* FUN_7b1c, immediately after the angle-accumulator-3D analog and
* immediately before `RW52 += RW3E` at 0x7B79. Reuses scratch_rpm /
* scratch_demand already populated by compute_angle_accumulator_3d
* earlier in the same tick.
*
* Both MULs are SIGNED (FE prefix). SHRAL is arithmetic; SHLL is
* logical.
*/
static void compute_angle_kick_2d(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x6A94–0x6AA3: prologue (PUSH RW1C/.../RW2A) — handled by C locals.
* 0x6AA4–0x6AAA: RW2A = RWC6 + 0x32 — points at the kick table cal slot. */
/* 0x6AAB–0x6AB6: PUSH #0x194 / #0x184 / [RW2A] / LCALL FUN_6f1e
* (T06215's combine_two_submaps_to_word analog).
* 2-D bilinear combine on (rpm, demand) over data_table_2d_kick.
* 0x6AB6: ST RW1C, RW3E — RW3E = bilinear result. */
int16_t bilinear = combine_two_submaps_to_word(cal->data_table_2d_kick,
&rt->scratch_rpm,
&rt->scratch_demand);
rt->angle_kick_2d = bilinear;
/* 0x6ABD–0x6AC2: MUL RL1C, RW42, [RWC6+0x34] — signed (FE prefix);
* 0x6AC3–0x6AC5: SHRAL RL1C, #0x8 (arithmetic).
* 0x6AC6–0x6AC8: ADD RW3E, RW1C — fold demand-weighted offset in. */
int32_t dem_prod = (int32_t)rt->angle_dec_cmd * (int32_t)cal->cal_rwc6_34;
int32_t dem_shifted = dem_prod >> 8; /* arithmetic */
int16_t dem_low = (int16_t)(dem_shifted & 0xFFFF);
rt->angle_kick_2d = (int16_t)(rt->angle_kick_2d + dem_low);
/* 0x6AC9–0x6ACF: MUL RL1C, RW3E, temp_comp — signed (FE prefix);
* 0x6AD0–0x6AD2: SHLL RL1C, #0x2 (LOGICAL); take HIGH word.
* 0x6AD3–0x6AD5: ST RW1E, RW3E — RW3E = high_word((RW3E × temp_comp_factor) << 2). */
int32_t scale_prod = (int32_t)rt->angle_kick_2d * (int32_t)rt->temp_comp_factor;
uint32_t scale_shifted = ((uint32_t)scale_prod) << 2; /* logical */
rt->angle_kick_2d = (int16_t)(uint16_t)(scale_shifted >> 16);
/* 0x6AD6–0x6AE6: epilogue. */
}
/*
* 1:1 translation of FUN_732d @ 0x732d–0x736d (T06211 variant).
*
* Produces RW3C (rt->accel_comp_offset). MUL at 0x7337 carries the FE
* prefix → SIGNED 16×16→32. SHLL at 0x733d is a logical shift-left
* by 4 on the 32-bit product — the effective result lands in the HIGH
* word (RW1E). Compared to T06031's FUN_5ecf the only differences are
* the calibration offsets used by the clamp: cal_78/cal_7a here vs.
* cal_7e/cal_80 in T06031.
*/
static void compute_accel_comp_offset(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x732d–0x7330: prologue (PUSH RW1C, RW1E) — handled by C locals */
/* 0x7331–0x7336: SUB RW1C, RW40, *(0x0138) (3-operand)
* rw1c = rpm − rpm_baseline. */
int16_t rw1c = (int16_t)((int16_t)rt->rpm - rt->rpm_baseline);
/* 0x7337–0x733c: MUL RL1C, *(0x0164) (FE prefix → SIGNED 16×16→32)
* RL1C = rw1c × accel_comp_gain. */
int32_t rl1c = (int32_t)rw1c * (int32_t)rt->accel_comp_gain;
/* 0x733d–0x733f: SHLL RL1C, #0x4
* Logical long shift-left by 4. Effective output is the HIGH word
* (RW1E) — that is what the clamp tests and what gets written. */
uint32_t u_rl1c = ((uint32_t)rl1c) << 4;
int16_t rw1e = (int16_t)(uint16_t)(u_rl1c >> 16);
/* 0x7340–0x7359: clamp against CAL[0x78] (upper) and CAL[0x7a] (lower).
* JLE / JGE are SIGNED. cal_78/cal_7a are signed 16-bit values. */
if (rw1e > cal->cal_78) {
rw1e = cal->cal_78;
} else if (rw1e < cal->cal_7a) {
rw1e = cal->cal_7a;
}
/* 0x735a–0x7362: enable gate on *(0x0226).
* CMPB ZRlo, *(0x0226); JNE LAB_7363 — fall-through (CLR RW1E) only
* when the byte is zero. */
if (rt->reset_gate_0226 == 0u) {
rw1e = 0;
}
/* 0x7363–0x7365: ST RW1E, RW3C — write accel_comp_offset. */
rt->accel_comp_offset = rw1e;
/* 0x7366–0x7368: ORB REC, #0x01 — set reset flag so the
* accumulator-inject block in FUN_7453 folds RW3C next tick. */
rt->rec = (uint8_t)(rt->rec | 0x01u);
/* 0x7369–0x736d: epilogue (POP RW1E, RW1C, RET) — handled by C */
(void)rw1c; /* rw1c is discarded after MUL; preserved for block-mapping */
}
/*
* 1:1 translation of FUN_5f33 @ 0x5f33–0x5f4d (T06215 ROM analog of
* T06031's FUN_736e). The "Try_calc_accel_offset" wrapper that conditionally
* calls compute_accel_comp_offset based on REC.0 / REC.1 / gate_0220.
*
* Triggered from the live-ROM Tooth_scheduler at 0x7abe, on the per-cylinder
* tooth event where R88 == cal_byte_56 — i.e. once per cylinder cycle,
* immediately after the rpm_baseline producer (Calculate_mid_rpm @ 0x51e1)
* updates *(0x0138). Wired into the public API as phi_per_cylinder_event.
*
* 0x5f37: ORB REC, #0x4 ; REC.2 := 1 (wrapper visited flag)
* 0x5f3a: JBS REC, 0x0, exit ; REC.0 set → fresh value pending, skip
* 0x5f3d: JBS REC, 0x1, compute ; REC.1 set → forced compute path
* 0x5f40: CMPB ZRlo, *(0x0220) ; gate_0220 (is_rpm_above_1000_hyst.)
* 0x5f45: JE exit ; gate_0220 == 0 → skip
* 0x5f47: SCALL FUN_5ef2 ; compute_accel_comp_offset
* 0x5f49: epilogue
*/
static void compute_accel_comp_offset_gated(runtime_state_t *rt,
const calibration_t *cal)
{
/* 0x5f37: REC |= 0x04 — set unconditionally, even on skip paths. */
rt->rec = (uint8_t)(rt->rec | 0x04u);
/* 0x5f3a: REC.0 set → consumer (FUN_7453) hasn't drained the previous
* RW3C yet; skip recomputation to avoid losing the pending value. */
if ((rt->rec & 0x01u) != 0u) {
return;
}
/* 0x5f3d–0x5f47: REC.1 forces the compute path; otherwise gate on
* the live-byte hysteresis at *(0x0220). */
if (((rt->rec & 0x02u) != 0u) || (rt->gate_0220 != 0u)) {
compute_accel_comp_offset(rt, cal);
}
}
/*
* 1:1 translation of the RPM-hysteresis block at 0x7801–0x7825 inside
* the orphan scheduler at 0x77FF (T06211 variant). Bytes were not
* auto-analyzed by Ghidra — verified via
* `python tools/mcs96_disasm.py 0x77F0 100 --variant T06211`.
*
* Updates `rt->gate_0220`:
* rpm < cal_74 → gate_0220 = 0 (deassert)
* rpm >= cal_74 + cal_76 → gate_0220 = 1 (assert)
* in between → gate_0220 unchanged (deadband)
*/
static void compute_gate_0220(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x7801–0x7806: unsigned compare rpm < cal_74. */
if ((uint16_t)rt->rpm < (uint16_t)cal->cal_74) {
rt->gate_0220 = 0u;
return;
}
/* 0x7808–0x7815: unsigned compare rpm < cal_74 + cal_76. */
uint16_t upper = (uint16_t)((uint16_t)cal->cal_74 + (uint16_t)cal->cal_76);
if ((uint16_t)rt->rpm < upper) {
/* Deadband — leave rt->gate_0220 unchanged. */
return;
}
/* 0x7817–0x781A. */
rt->gate_0220 = 1u;
}
/*
* 1:1 translation of FUN_62a2 @ 0x62a2–0x6318 (T06211 variant).
*
* Per-tick state machine producing RW7A (rt->tein_valve_fault_guard).
* Two phases driven by a byte-state machine:
* Phase 1 (scratch_010f == 0): RW7A unconditionally cleared, counter
* scratch_010e ramps under slow timing (RWC2 <= dat_604c) and
* decays under fast timing. On firing (counter ≥ scratch_0103),
* RW7A = cal_48 and we advance to Phase 2.
* Phase 2 (scratch_010f != 0): RW7A persists; state byte decays
* under fast timing or re-loads from scratch_0108 under slow
* timing.
*
* Peripheral writes (OR RWEA, #0x200 at 0x62cd) are NO-OPS in the C
* translation — diagnostics fault channel only.
*/
static void compute_tein_valve_fault_guard(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x62a2–0x62a3: prologue (PUSH RW1C) — handled by C locals */
/* 0x62a4–0x62a8: CMPB RF5, #0x0 ; JE LAB_6316
* Early-out if RF5 (peripheral input gate) is zero. */
if (rt->rf5_gate == 0u) {
return;
}
/* 0x62a9–0x62af: CMPB ZRlo, *(0x0221) ; JE LAB_6316
* Early-out if *(0x0221) (peripheral output flag set by FUN_5a97)
* is zero. */
if (rt->scratch_0221 == 0u) {
return;
}
/* 0x62b0–0x62b6: CMPB ZRlo, *(0x010f) ; JNE LAB_62fc
* Branch on Phase-2 state byte. JNE jumps if *(0x010f) != 0 → Phase 2.
* Fall through (==0) → Phase 1. */
if (rt->scratch_010f != 0u) {
goto phase_2;
}
/* ────────────────────────────────────────────────────────────────
* Phase 1 (state == 0): arming / counter ramp / firing
* ────────────────────────────────────────────────────────────────
*/
/* 0x62b7–0x62bb: LDB R1C, *(0x010e) — load Phase-1 counter into r1c. */
uint8_t r1c = rt->scratch_010e;
/* 0x62bc–0x62be: ST ZR, RW7A — RW7A unconditionally cleared in Phase 1. */
rt->tein_valve_fault_guard = 0;
/* 0x62bf–0x62c5: CMP RWC2, dat_604c ; JH LAB_62ee (UNSIGNED).
* JH = unsigned >. If RWC2 > dat_604c → Phase-1 decay branch
* (LAB_62ee). Otherwise fall through to the count-up branch. */
if (rt->rwc2 > (uint16_t)cal->dat_604c) {
goto phase_1_decay;
}
/* 0x62c6–0x62cc: ADDB R1C, CAL_byte[RWA4]+0x9c ; INCB R1C
* r1c = scratch_010e + cal_byte_9c + 1, with byte wrap-around. */
r1c = (uint8_t)(r1c + cal->cal_byte_9c);
r1c = (uint8_t)(r1c + 1u);
/* 0x62cd–0x62d0: OR RWEA, #0x200
* Peripheral status bit 9 set — fault/diag channel, not modelled. */
/* 0x62d1–0x62d7: CMPB R1C, *(0x0103) ; JNC LAB_62ec (UNSIGNED).
* JNC = unsigned <. If r1c < scratch_0103 → counter not yet at
* threshold, persist r1c and exit. Otherwise fall through to fire. */
if (r1c >= rt->scratch_0103) {
/* 0x62d8–0x62dc: LD RW1C, CAL[RWA4]+0x48
* 0x62dd–0x62df: ST RW1C, RW7A — THE NON-ZERO WRITE. */
rt->tein_valve_fault_guard = cal->cal_48;
/* 0x62e0–0x62e4: LDB R1C, *(0x0108) — load Phase-2 init value. */
r1c = rt->scratch_0108;
/* 0x62e5–0x62e9: STB R1C, *(0x010f) — advance to Phase 2. */
rt->scratch_010f = r1c;
/* 0x62ea–0x62eb: CLRB R1C — counter reset (note: r1c here is
* the LOCAL working byte, not scratch_010e; the persisted
* counter is set to 0 via the STB at 0x62f5 which uses this r1c). */
r1c = 0u;
}
/* 0x62ec–0x62ed: SJMP LAB_62f5 — fall through to counter store. */
goto phase_1_store_counter;
phase_1_decay:
/* 0x62ee–0x62f4: CMPB ZRlo, R1C ; JE LAB_62f5 ; DECB R1C
* Decay branch: if r1c (= scratch_010e on entry) is nonzero,
* decrement it. Saturated at zero. */
if (r1c != 0u) {
r1c = (uint8_t)(r1c - 1u);
}
phase_1_store_counter:
/* 0x62f5–0x62f9: STB R1C, *(0x010e) — persist the (possibly
* incremented or decremented) counter value. */
rt->scratch_010e = r1c;
/* 0x62fa–0x62fb: SJMP LAB_6316 — exit. */
return;
phase_2:
/* ────────────────────────────────────────────────────────────────
* Phase 2 (state != 0): RW7A holds; state decays / re-loads
* ────────────────────────────────────────────────────────────────
*/
{
/* 0x62fc–0x6300: LDB R1C, *(0x010f) — load Phase-2 state. */
uint8_t r1c2 = rt->scratch_010f;
/* 0x6301–0x6307: CMP RWC2, dat_604c ; JNH LAB_630c (UNSIGNED).
* JNH = unsigned <=. If RWC2 <= dat_604c → re-load to max.
* Otherwise (RWC2 > dat_604c) → decrement state. */
if (rt->rwc2 > (uint16_t)cal->dat_604c) {
/* 0x6308–0x6309: DECB R1C — state decays under fast timing. */
r1c2 = (uint8_t)(r1c2 - 1u);
} else {
/* 0x630c–0x6310: LDB R1C, *(0x0108) — re-load to max under
* slow timing. */
r1c2 = rt->scratch_0108;
}
/* 0x6311–0x6315: STB R1C, *(0x010f) — persist new state. */
rt->scratch_010f = r1c2;
}
/* 0x6316–0x6318: epilogue (POP RW1C ; RET) — handled by C */
}
/*
* Translation of FUN_7453 @ 0x7453–0x74f9 (T06211 variant), scoped to
* RW48 production. Sole writer of target_inj_angle (RW48) at 0x749f
* and target_eoi (RW5A) at 0x74a2.
*
* The upper clamp at 0x7493 is one-sided — no lower clamp — so
* negative results propagate. This matches the assembly exactly and
* must not be "fixed" by adding a lower clamp.
*
* The ROM common-tail (0x74bc–0x74f4 — RW54/RW56/R8B + FUN_739f /
* FUN_5a97 sub-call gate) is omitted: it computes an EPA-scheduling
* lead correction from RW6E (the reluctor tooth-period measurement)
* that never feeds RW48.
*/
static void compute_target_injection_angle(runtime_state_t *rt, const calibration_t *cal)
{
/* 0x7453–0x7456: prologue (PUSH RW1C, RW1E) — handled by C locals */
int16_t rw1c;
int16_t rw1e;
/* 0x7457–0x745a: CMP ZR, RW44 / JE LAB_74b6
* Early-out: if inj_qty_demand == 0, skip the main computation.
* target_inj_angle and target_eoi are NOT updated on this path —
* they retain the previous tick's value. */
if (rt->inj_qty_demand == 0) {
return;
}
/* 0x745c–0x7463: JBC REC, 0x0, LAB_7465
* If REC.0 (reset_flag) is set, accumulate accel_comp_offset into
* angle_accumulator and clear the ENTIRE REC byte (STB ZRlo, REC).
* T06031's analog at 0x8aad only cleared bit 0 via CLRB — T06211
* zeros the whole byte here. Preserve that. */
if ((rt->rec & 0x01u) != 0u) {
rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->accel_comp_offset);
rt->rec = 0u;
}
/* 0x7465–0x7474: CMPB ZRlo, DAT_0200 / JE / ADD RW52, DAT_0200 / ST ZR, DAT_0200
* Event-accumulator drain: if LOW BYTE of scratch_0200 is nonzero,
* fold the whole WORD into angle_accumulator and clear. The CMPB
* tests ONLY the low byte — a word with just the high byte set
* would skip (matches T06031's pattern). */
if ((uint8_t)((uint16_t)rt->scratch_0200 & 0xFFu) != 0u) {
rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->scratch_0200);
rt->scratch_0200 = 0;
}
/* 0x7476–0x747a: JBC R53, 0x7, LAB_747b / CLR RW52
* Saturate angle_accumulator to zero when negative. Assembly tests
* bit 7 of R53 (high byte of RW52) — the sign bit of the signed
* word. If set (value is negative), clear the whole word. */
if (rt->angle_accumulator < 0) {
rt->angle_accumulator = 0;
}
/* 0x747b–0x7480: ADD RW1C, RW7A, CAL[RWA4]+0x1e
* 3-operand ADD: rw1c = tein_valve_fault_guard + tein_nominal. */
rw1c = (int16_t)(rt->tein_valve_fault_guard + cal->tein_nominal);
/* 0x7481–0x7485: ADD RW1C, 0x11a (2-op)
* rw1c += tein_overtemp_guard (produced by FUN_5ca1 @ 0x5d3e). */
rw1c = (int16_t)(rw1c + rt->tein_overtemp_guard);
/* 0x7486–0x7488: MULU RL1C, RW40 — UNSIGNED 16x16 → 32
* 0x7489–0x748b: SHLL RL1C, #0x1 — logical long shift-left by 1
* After these two instructions RL1C = (rw1c × rpm) << 1. The low
* word (rw1c) is discarded by the next 3-op ADD at 0x748c; what
* matters is the HIGH word, which lands in RW1E and is subtracted
* from the new rw1c at 0x7490. */
{
uint32_t rl1c = (uint32_t)(uint16_t)rw1c * (uint32_t)rt->rpm;
rl1c = rl1c << 1;
rw1e = (int16_t)(uint16_t)(rl1c >> 16);
/* low word of rl1c is overwritten by the next ADD */
}
/* 0x748c–0x748f: ADD RW1C, RW52, RW42 (3-op)
* rw1c = angle_accumulator + angle_dec_cmd. */
rw1c = (int16_t)(rt->angle_accumulator + rt->angle_dec_cmd);
/* 0x7490–0x7492: SUB RW1C, RW1E (2-op)
* Subtract the rpm-scaled fractional term. */
rw1c = (int16_t)(rw1c - rw1e);
/* 0x7493–0x749e: CMP RW1C, CAL[RWA4]+0x54 / JLE LAB_749f / LD RW1C, CAL
* Upper-clamp only. JLE is SIGNED. No lower clamp — negative
* results pass through, carrying the "target invalid" sign bit. */
if (rw1c > cal->cal_54) {
rw1c = cal->cal_54;
}
/* 0x749f–0x74a1: LD RW48, RW1C — THE WRITE (sole writer of RW48). */
rt->target_inj_angle = rw1c;
/* 0x74a2–0x74a7: ADD RW5A, RW1C, 0x14e (3-op, TABLE[ZR])
* target_eoi = target_inj_angle + phi1, where phi1 is the precomputed
* cell at *(0x014e). The C model recomputes phi1 = phi0 + dphi inline
* (see variants/T06215/docs/open-questions.md §6). */
{
int16_t phi1 = (int16_t)(cal->phi0 + rt->dphi);
rt->target_eoi = (int16_t)(rw1c + phi1);
}
/* 0x74a8–0x74f4 (common-tail RW54/RW56/R8B + FUN_739f/FUN_5a97
* sub-call gate) is scoped out — see function docstring. */
/* 0x74f5–0x74f9: epilogue (POP RW1E, RW1C, RET) — handled by C */
}
/* ─── Public lifecycle ──────────────────────────────────────────── */
void phi_init(phi_state_t *state, const phi_cal_t *cal,
const phi_input_getters_t *getters)
{
memset(state, 0, sizeof(*state));
state->getters = getters;
/* Bind cal descriptors. Cast away const because submap_descriptor_t's
* input_var field is runtime-bound by design (the C model treats the
* cal as a writable buffer, not flash-resident, since extract_*.py
* emits a non-const initializer for this very reason). */
phi_t06215_bind_inputs(&state->rt, (phi_cal_t *)cal);
/* Pull boot-only getters — Phase-1 / Phase-2 thresholds for the
* FUN_62a2 state machine. */
state->rt.scratch_0103 = getters->get_scratch_0103();
state->rt.scratch_0108 = getters->get_scratch_0108();
/* Per-cylinder dispatch bytes r88/r89 and the scratch_01ae table
* only feed the ROM common-tail (RW54/RW56/R8B + FUN_739f/FUN_5a97
* sub-call gate), which never reaches RW48 — see
* compute_target_injection_angle.c. Scoped out of this port. */
/* Seed the IIR prescaler so the 1 kHz hook starts a fresh count from
* boot — mirrors Timer_1khz LDB R94, DAT_6061 at 0x7a26. memset above
* already zeroed rt->rw9c / rt->rw9e (mirrors FUN_6ba3 @ 0x6bbf–0x6bc1). */
state->rt.temp_phi_comp_r94 = 100u;
/* Bake scratch_0221 = 1 — FUN_62a2's early-out gate. In the live
* ROM this is written by FUN_5a97 with an RPM-correlated condition
* via RW68 (whose actual writer lives in the unreadable ROM gap
* 0x848e–0x9bbf). Modeling that loop is unnecessary because RW7A
* is monotonic: the only RW48-relevant effect of scratch_0221 is
* "did Phase 1→2 ever latch", and live data shows scratch_0221=1
* during cranking when that latching occurs. Above ~1400 rpm the
* live ECU flips it to 0 to freeze a counter that's already
* latched — observationally inert. See
* variants/T06211/docs/scratch_0221_writer.md. */
state->rt.scratch_0221 = 1u;
}
void phi_service(phi_state_t *state, const phi_cal_t *cal, phi_outputs_t *out)
{
runtime_state_t *rt = &state->rt;
const phi_input_getters_t *g = state->getters;
/* ── Stage 1: pull per-tick external inputs ─────────────────────
* These all originate outside the angle-calculation chain. See
* variants/T06211/docs/out-of-scope.md for the off-chain producers
* that drive each one. */
rt->rpm = g->get_rpm();
rt->inj_qty_demand = g->get_inj_qty_demand();
rt->angle_dec_cmd = g->get_angle_dec_cmd();
rt->temperature = g->get_temperature();
/* rt->rpm_baseline is intentionally NOT pulled here — its cadence is
* tooth-13-event-driven, not main-loop-driven. The pull happens at
* the two consumer-side entry points instead:
* - phi_per_cylinder_event (the per-cylinder hook)
* - just before compute_angle_accumulator_3d in Stage 3b (the
* PTS-event fallback path's embedded Alternate_phiad_calc gate).
* See the comment at each pull site. */
rt->rwc2 = g->get_rwc2();
rt->reset_gate_0226 = g->get_reset_gate_0226();
rt->dphi = g->get_dphi();
/* rt->rw9e is produced internally by compute_temp_phi_comp on its
* 10 Hz cadence; it is not pulled from a getter. The host drives the
* 1 kHz hook compute_temp_phi_comp_tick_1khz from a separate timer. */
/* Per-tick constants — hard-wire chain inputs that the live ECU drives
* but the simulation pins to a single value:
* rf5_gate = 1 → FUN_62a2 peripheral input gate is permanently
* active in the live ECU; it does not gate the angle
* pipeline in any observable way. */
rt->rf5_gate = 1u;
/* Snapshot REC.0 BEFORE FUN_7453 runs — the function clears the
* whole REC byte at 0x7462 if the bit was set, so we have to grab
* it now to expose the "accel_comp_offset folded this tick" flag
* downstream. */
uint8_t rec0_entering_fun7453;
/* ── Stage 2: compute_tein_overtemp_guard (FUN_5ca1 @ 0x5ca1) ──────────
* Produces rt->tein_overtemp_guard (the seed addend folded into RW1C inside
* FUN_7453 at 0x7481) from rt->temperature and rt->rpm. The ROM's
* sensor-raw temperature scaler (0x5ca7–0x5cf6) is scoped out — see
* compute_tein_overtemp_guard.c. */
compute_tein_overtemp_guard(rt, cal);
/* ── Stage 3: compute_accel_comp_gain (FUN_72b0 @ 0x72b0) ───────
* 3-submap pipeline (RPM × demand → 2-D combine; temperature → 1-D
* refine; signed MUL high-word → *(0x0164) = rt->accel_comp_gain).
* Always called here; in the ROM the orphan @ 0x77ff gates this on
* a freshly-updated gate_0220, but our simulation runs the producer
* unconditionally so the gain is fresh each tick. */
compute_accel_comp_gain(rt, cal);
/* ── Stage 3b: compute_angle_accumulator_3d (FUN_722e @ 0x722e) ────
* Three RWC6-relative submap evals (RPM, demand, angle_dec_cmd) through
* FUN_7092 (3-D trilinear = combine_three_submaps_to_word) → writes
* rt->angle_accumulator (RW52) and rt->scratch_0158 debug mirror.
* Also populates rt->scratch_rpm and rt->scratch_demand which the
* angle-kick stage (3d) reuses without re-evaluating.
*
* The Alternate_phiad_calc embedded gate at the tail of this function
* may fire compute_accel_comp_offset (the "PTS-event fallback path").
* Pull rpm_baseline FRESH right before this gate so the producer reads
* the latest tooth-13 snapshot rather than whatever stale value Stage
* 1 left behind — phi_service's main-loop cadence is not synchronised
* with the per-cylinder tooth-13 event, so a Stage-1 pull may capture
* the previous cycle's baseline and invert RW3C polarity. Mirrors the
* pull at the top of phi_per_cylinder_event so both producer entry
* points see the same snapshot ordering. */
rt->rpm_baseline = g->get_rpm_baseline();
compute_angle_accumulator_3d(rt, cal);
/* ── Stage 3c: compute_temp_comp_factor (orphan calc_temp_comp_factor @ 0x6AE7) ─
* Per-tick rebuild of *(0x02FC) in T06215 (T06211: *(0x02F4)) from rpm,
* temperature, the two boot switches (cal_temp_comp_switch_complete /
* _dynamic), and the external rw9e state input. Mirrors the orphan call
* at 0x7B14 that fires before the angle scheduler at FUN_7b1c. */
compute_temp_comp_factor(rt, cal);
/* ── Stage 3d: compute_angle_kick_2d (orphan @ 0x6A94) ─────────────
* 2-D bilinear kick over the table at *(RWC6+0x32), reusing the
* scratches populated by Stage 3b, plus a demand-weighted offset
* `(angle_dec_cmd × cal_rwc6_34) >> 8`, post-scaled by
* temp_comp_factor (Stage 3c output). Mirrors `LCALL compute_angle_kick_2d`
* at 0x7B76. Writes rt->angle_kick_2d (RW3E). */
compute_angle_kick_2d(rt, cal);
/* ── Stage 3e: fold the kick into the accumulator + saturate ──────
* Mirrors `ADD RW52, RW3E` at 0x7B79 followed by the `JBC R53.7 /
* CLR RW52` saturate at 0x7B7C–0x7B7F. This happens BEFORE
* compute_target_injection_angle (FUN_7453 analog @ 0x754D);
* FUN_7453's own REC.0 fold of accel_comp_offset (RW3C) and
* *(0x0200) drain run later inside Stage 7. */
rt->angle_accumulator = (int16_t)(rt->angle_accumulator + rt->angle_kick_2d);
if (rt->angle_accumulator < 0) {
rt->angle_accumulator = 0;
}
/* ── Stage 4: compute_accel_comp_offset is no longer called here ──
* The producer is now driven by the live-ROM cadence:
*
* 1. Per-cylinder path — host calls phi_per_cylinder_event() at the
* cal_byte_56 tooth (mirrors Tooth_scheduler @ 0x7abe). This
* sets REC.2 and may run compute_accel_comp_offset depending on
* gate_0220 / REC.1 state.
*
* 2. PTS-event fallback path — the Alternate_phiad_calc embedded
* gate at the tail of compute_angle_accumulator_3d (Stage 3b)
* runs compute_accel_comp_offset when REC.2 is set AND REC.0
* is clear.
*
* Calling the bare producer here would overwrite RW3C every tick
* regardless of cadence, which is the pre-2026-05-06 divergence the
* live-ECU comparison surfaced. */
/* ── Stage 5: compute_gate_0220 (orphan @ 0x77ff) ───────────────
* Updates rt->gate_0220 from current rpm with hysteresis bands at
* cal_74 / cal_74+cal_76. Surfaced in the outputs for diagnostic
* tracing; the bare compute_accel_comp_offset above ignores it. */
compute_gate_0220(rt, cal);
/* ── Stage 6: compute_tein_valve_fault_guard (FUN_62a2 @ 0x62a2) ───
* Phase-1/Phase-2 byte-state machine producing RW7A. Uses rwc2
* vs cal->dat_604c to drive timing. RW7A is added to tein_nominal and
* tein_overtemp_guard inside FUN_7453 at 0x747b/0x7481. */
compute_tein_valve_fault_guard(rt, cal);
/* Snapshot REC.0 right before FUN_7453 — used downstream for the
* output flags byte (FUN_7453 clears the whole REC byte at 0x7462
* if bit 0 was set, so we'd lose this evidence otherwise). */
rec0_entering_fun7453 = (uint8_t)(rt->rec & 0x01u);
/* ── Stage 7: compute_target_injection_angle (FUN_7453 @ 0x7453) ──
* Final producer of RW48 (target_inj_angle) and RW5A (target_eoi).
* Runs the inj_qty_demand == 0 alt path internally; on the alt path
* target_inj_angle and target_eoi retain their prior-tick values
* (RW50/activation_angle, the ROM's alt-path source, is not modeled
* — see compute_target_injection_angle.c). The ROM common-tail
* (RW54/RW56/R8B + FUN_739f/FUN_5a97) is scoped out — its DIVU
* uses RW6E (the reluctor tooth period in μs) to produce an
* EPA-scheduling firing-time lead correction that never feeds
* RW48. */
compute_target_injection_angle(rt, cal);
/* ── Populate outputs ─────────────────────────────────────────── */
out->target_inj_angle = rt->target_inj_angle;
out->target_eoi = rt->target_eoi;
out->angle_accumulator = rt->angle_accumulator;
out->accel_comp_offset = rt->accel_comp_offset;
out->accel_comp_gain = rt->accel_comp_gain;
out->tein_valve_fault_guard = rt->tein_valve_fault_guard;
out->tein_overtemp_guard = rt->tein_overtemp_guard;
out->temperature = rt->temperature;
out->temp_comp_factor = rt->temp_comp_factor;
out->angle_kick_2d = rt->angle_kick_2d;
out->gate_0220 = rt->gate_0220;
out->flags = (uint8_t)(((rt->target_inj_angle < 0) ? 0x01u : 0u)
| ((rec0_entering_fun7453 != 0u) ? 0x02u : 0u)
| ((rt->gate_0220 != 0u) ? 0x04u : 0u));
}
size_t phi_state_size(void) { return sizeof(phi_state_t); }
size_t phi_cal_size(void) { return sizeof(phi_cal_t); }
/*
* phi_per_cylinder_event — public per-cylinder hook.
*
* Models the live-ROM `Tooth_scheduler` dispatch at 0x7abe:
*
* if (R88 == cal_byte_56) { // 0x7aa9 JE LAB_7abb
* Calculate_mid_rpm(); // 0x7abb LCALL FUN_51e1
* Try_calc_accel_offset(); // 0x7abe LCALL FUN_5f33
* }
*
* The host is responsible for the `Calculate_mid_rpm` analog: write the
* fresh rpm_baseline (RW138) into `state->rt.rpm_baseline` (or via the
* getter) BEFORE calling this hook. This routine then runs the
* Try_calc_accel_offset wrapper (compute_accel_comp_offset_gated), which
* sets REC.2 and conditionally calls compute_accel_comp_offset.
*
* Cadence (per the live-ECU side-by-side traces):
* - High RPM (gate_0220 == 1): hook fires the producer; REC.0 is set;
* phi_service's Stage 3b fallback gate observes REC.0 and skips.
* - Low RPM (gate_0220 == 0): hook only sets REC.2; phi_service's
* Stage 3b runs the producer because REC.1 was set there and REC.0
* is still clear.
* - In both cases compute_target_injection_angle (Stage 7) clears the
* entire REC byte after consuming RW3C, so REC reads back as 0
* between events — matching the live observation that *(0x00ec)
* stays at zero on the bus probe.
*
* Reentrant per phi_state_t. Safe to invoke from a tooth-edge ISR
* provided the host serialises against phi_service.
*
* Pulls rpm_baseline from the getter at hook entry — the host computes
* the fresh tooth-13 snapshot just before invoking this hook, but
* phi_service's Stage 1 pull captures rpm_baseline at main-loop cadence
* (independent of tooth-13 events), leaving rt->rpm_baseline stale at
* this moment. The fresh pull here guarantees the producer sees the
* just-computed snapshot. The mirror pull just before Stage 3b in
* phi_service does the same for the PTS-event fallback path.
*/
void phi_per_cylinder_event(phi_state_t *state, const phi_cal_t *cal)
{
state->rt.rpm_baseline = state->getters->get_rpm_baseline();
compute_accel_comp_offset_gated(&state->rt, cal);
}
Reference in New Issue View Git Blame Copy Permalink