135 lines
4.8 KiB
C
135 lines
4.8 KiB
C
/*
|
||
* ckp_acquisition.c
|
||
*
|
||
* Created on: May 5, 2026
|
||
* Author: herli
|
||
*
|
||
* Mirrors ROM FUN_55e0 @ 0x55e0 → FUN_6a4b @ 0x6a4b for the CKP-zero
|
||
* acquisition path. See ckp_acquisition.h for the data-flow summary.
|
||
*/
|
||
|
||
#include "ckp_acquisition.h"
|
||
#include "pwm.h" /* pwm_cal_rom + shra16 */
|
||
#include "ee_manager.h"
|
||
|
||
/* Inputs */
|
||
int16_t B_CKP_OFFSET = 0;
|
||
uint8_t commitCKP_offset = 0;
|
||
|
||
/* RAM trims (DAT_0430 word, DAT_0404 signed byte) — see header. */
|
||
int16_t CKP_RAM_TRIM_0430 = 0;
|
||
|
||
/* Outputs */
|
||
int16_t dCKP_OFFSET = 0;
|
||
int16_t CKP_ZERO_OFFSET = 0;
|
||
|
||
/* FUN_6a4b @ 0x6a4b core — DAT_0152 derivation only.
|
||
*
|
||
* ROM body (fulldissasembly.txt:20025–20061):
|
||
* RW1E = cal[+0x50] + DAT_0430
|
||
* RW1C = RW1E - DAT_0434 - cal[+0x52] + (int8_t)DAT_0404
|
||
* while RW1C < 0 : RW1C += cal[+0xa2] ; JGE LAB_6a7e (signed)
|
||
* while RW1C > cal[+0xa2] : RW1C -= cal[+0xa2] ; JLE LAB_6a8c (signed)
|
||
* DAT_0152 = RW1C
|
||
*
|
||
* Side-effects (FUN_6c70 R8F/R90 derivation, DAT_02cb/R2C/R2D clears,
|
||
* DAT_017a=5, DAT_0278 reload) belong to the CKP-decode synchronization
|
||
* stage and are intentionally not ported here — see the plan's
|
||
* "Out of scope" section.
|
||
*/
|
||
static void ckp_recompute_zero_offset(void)
|
||
{
|
||
int16_t anchor = (int16_t)((int16_t)pwm_cal_rom.ckp_zero_anchor
|
||
+ CKP_RAM_TRIM_0430);
|
||
|
||
/* LDBSE at fulldissasembly.txt:20038 sign-extends the byte to a
|
||
* word before the ADD. */
|
||
int16_t v = (int16_t)(anchor
|
||
- dCKP_OFFSET
|
||
- pwm_cal_rom.can_dckp_offset_bias
|
||
+ (int16_t)s_dfi_code);
|
||
|
||
int16_t mod = pwm_cal_rom.ckp_modulus;
|
||
|
||
/* Verbatim ROM modulo. JGE/JLE in the ROM are signed compares, so
|
||
* the loops bracket v into [0, mod] inclusive. Replacing with `%`
|
||
* would diverge for negative v under C90/C99 implementation-defined
|
||
* sign-of-remainder rules. */
|
||
while (v < 0) v = (int16_t)(v + mod);
|
||
while (v > mod) v = (int16_t)(v - mod);
|
||
|
||
CKP_ZERO_OFFSET = v;
|
||
}
|
||
|
||
/* FUN_55e0 @ 0x55e0 entry — detect change in (B_CKP_OFFSET >> 1) and
|
||
* update dCKP_OFFSET; on change, FUN_6a4b is invoked. We always re-run
|
||
* FUN_6a4b on the first call so consumers see a coherent
|
||
* CKP_ZERO_OFFSET even before B_CKP_OFFSET first moves.
|
||
*/
|
||
int16_t get_ckp_zero(void)
|
||
{
|
||
static uint8_t initialized = 0;
|
||
|
||
int16_t halved = shra16(B_CKP_OFFSET, 1);
|
||
int16_t biased_now = (int16_t)(dCKP_OFFSET
|
||
+ pwm_cal_rom.can_dckp_offset_bias);
|
||
|
||
if (halved != biased_now) {
|
||
dCKP_OFFSET = (int16_t)(halved
|
||
- pwm_cal_rom.can_dckp_offset_bias);
|
||
/* commitCKP_offset stays unused per user direction; future hook
|
||
* for the FUN_56d8 flash-write sequence and the DAT_01e7 bit-7
|
||
* persist-pending flag. */
|
||
//(void)commitCKP_offset;
|
||
ckp_recompute_zero_offset();
|
||
initialized = 1;
|
||
} else if (!initialized) {
|
||
ckp_recompute_zero_offset();
|
||
initialized = 1;
|
||
}
|
||
|
||
return CKP_ZERO_OFFSET;
|
||
}
|
||
|
||
/* FUN_6c70 @ 0x6c70 — process-tooth derivation (R90 byte store path).
|
||
*
|
||
* Translates fulldissasembly.txt:20453–20471 (the second half of FUN_6c70
|
||
* after the R8F clamp; only the R90 byte path is reproduced here since
|
||
* R8F is computed by the caller from word[0x152] high byte directly):
|
||
*
|
||
* 6c8b LD RW1C, [0x152] ; CKP_ZERO_OFFSET
|
||
* 6c90 ADD RW1C, cal+0x140 ; ckp_advance_per_tick
|
||
* 6c95 INCB R1D ; high byte += 1
|
||
* 6c97 CMPB R1D, cal+0x09F ; ckp_seg_wrap_threshold (=29)
|
||
* 6c9c JLE LAB_6ca5
|
||
* 6c9e SUB RW1C, cal+0x0A2 ; ckp_modulus (low byte=0 → R1D-=30)
|
||
* 6ca3 SJMP LAB_6cae
|
||
* LAB_6ca5:
|
||
* 6ca5 CMPB R1D, cal+0x0A0 ; ckp_teeth_per_seg (=26)
|
||
* 6caa JLE LAB_6cae
|
||
* 6cac CLRB R1D
|
||
* LAB_6cae:
|
||
* 6cae STB R1D, R90
|
||
*
|
||
* The word SUB at 6c9e operates on RW1C, but cal+0x0A2 = 0x1E00 has a
|
||
* zero low byte, so the high byte (R1D) decreases by exactly
|
||
* (ckp_modulus >> 8) = 30 with no borrow into the low byte. We
|
||
* therefore reproduce the byte-level effect directly without keeping
|
||
* the word around — RW1C is discarded by the ROM after RET anyway,
|
||
* since only the byte stored to R90 escapes the routine.
|
||
*/
|
||
uint8_t get_ckp_process_tooth(void)
|
||
{
|
||
uint16_t advanced = (uint16_t)((uint16_t)CKP_ZERO_OFFSET
|
||
+ (uint16_t)pwm_cal_rom.ckp_advance_per_tick);
|
||
uint8_t tooth = (uint8_t)((advanced >> 8) + 1u);
|
||
|
||
uint8_t modulus_high = (uint8_t)(((uint16_t)pwm_cal_rom.ckp_modulus) >> 8);
|
||
if (tooth > pwm_cal_rom.ckp_seg_wrap_threshold) {
|
||
tooth = (uint8_t)(tooth - modulus_high);
|
||
} else if (tooth > pwm_cal_rom.ckp_teeth_per_seg) {
|
||
tooth = 0u;
|
||
}
|
||
return tooth;
|
||
}
|