124 lines
4.1 KiB
C
124 lines
4.1 KiB
C
/*
|
||
* ckp_acquisition.c (variant/T06215/compact_src)
|
||
*
|
||
* Mirrors ROM Stage-1 caller @ 0x6330 (LCALL FUN_70d8) → FUN_70d8 @ 0x70d8
|
||
* for the CKP-zero acquisition path. Body verbatim from T06235; only the
|
||
* cal-field bindings (ckp_zero_anchor / can_dckp_offset_bias / ckp_modulus)
|
||
* carry T06215-specific cal slots — see pwm_addr_map.h.
|
||
*
|
||
* Stage-1 caller sequence (T06215 ROM 0x62b0..0x6332):
|
||
* RW1E = dCKP_OFFSET + cal+0x50 ; biased current
|
||
* RW1C = SHRA(B_CKP_OFFSET, 1) ; signed half
|
||
* if RW1C != RW1E:
|
||
* DAT_01e7 |= 0x80
|
||
* dCKP_OFFSET = RW1C - cal+0x50
|
||
* (flash commit gated by AD_resultlo == cal+0xCD)
|
||
* DAT_01e7 &= 0x7F
|
||
* LCALL FUN_70d8
|
||
*
|
||
* Stage-2 body (FUN_70d8 @ 0x70d8..0x7128):
|
||
* RW1E = cal+0x4e + DAT_0430
|
||
* v = RW1E - DAT_0434 - cal+0x50 + (int8_t)DAT_0404
|
||
* while v < 0 : v += cal+0xa0 ; signed JGE
|
||
* while v > cal+0xa0 : v -= cal+0xa0 ; signed JLE
|
||
* DAT_0152 = v
|
||
* DAT_0150 = cal+0x4c - RW1E
|
||
*/
|
||
|
||
#include "ckp_acquisition.h"
|
||
#include "pwm.h"
|
||
#include "ee_manager.h"
|
||
|
||
/* Inputs */
|
||
int16_t B_CKP_OFFSET = 0;
|
||
uint8_t commitCKP_offset = 0;
|
||
|
||
/* RAM trims */
|
||
int16_t CKP_RAM_TRIM_0430 = 0;
|
||
|
||
/* Outputs */
|
||
int16_t dCKP_OFFSET = 0;
|
||
int16_t CKP_ZERO_OFFSET = 0;
|
||
|
||
static void ckp_recompute_zero_offset(void)
|
||
{
|
||
int16_t anchor = (int16_t)((int16_t)pwm_cal_rom.ckp_zero_anchor
|
||
+ CKP_RAM_TRIM_0430);
|
||
|
||
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;
|
||
|
||
while (v < 0) v = (int16_t)(v + mod);
|
||
while (v > mod) v = (int16_t)(v - mod);
|
||
|
||
CKP_ZERO_OFFSET = v;
|
||
}
|
||
|
||
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);
|
||
ckp_recompute_zero_offset();
|
||
initialized = 1;
|
||
} else if (!initialized) {
|
||
ckp_recompute_zero_offset();
|
||
initialized = 1;
|
||
}
|
||
|
||
return CKP_ZERO_OFFSET;
|
||
}
|
||
|
||
/* FUN_7293 @ 0x7293 — process-tooth derivation (R90 byte store path).
|
||
*
|
||
* Translates t06215 dissasembly.txt:21346–21364 (the second half of
|
||
* FUN_7293 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):
|
||
*
|
||
* 72ae LD RW1C, [0x152] ; CKP_ZERO_OFFSET
|
||
* 72b3 ADD RW1C, cal+0x12C ; ckp_advance_per_tick
|
||
* 72b8 INCB R1D ; high byte += 1
|
||
* 72ba CMPB R1D, cal+0x09D ; ckp_seg_wrap_threshold (=29)
|
||
* 72bf JLE LAB_72c8
|
||
* 72c1 SUB RW1C, cal+0x0A0 ; ckp_modulus (low byte=0 → R1D-=30)
|
||
* 72c6 SJMP LAB_72d1
|
||
* LAB_72c8:
|
||
* 72c8 CMPB R1D, cal+0x09E ; ckp_teeth_per_seg (=26)
|
||
* 72cd JLE LAB_72d1
|
||
* 72cf CLRB R1D
|
||
* LAB_72d1:
|
||
* 72d1 STB R1D, R90
|
||
*
|
||
* The word SUB at 72c1 operates on RW1C, but cal+0x0A0 = 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;
|
||
}
|