166 lines
4.0 KiB
C
166 lines
4.0 KiB
C
/*
|
|
* ford_immo.c
|
|
*
|
|
* Created on: Nov 21, 2025
|
|
* Author: herli
|
|
*/
|
|
#include "ford_immo.h"
|
|
#include "timeouts.h"
|
|
#include "can_port.h"
|
|
|
|
|
|
uint16_t FIEONA_FIRSTWORD = 0x2001;
|
|
uint16_t FIEONA_SECONDWORD = 0x2000;
|
|
|
|
static uint16_t _immo_next2 = 0x4801; // predicted next rx = f(0x2001)
|
|
static uint16_t _rx_received = 0;
|
|
static uint8_t _rx_fresh = 0;
|
|
|
|
// --- Core step function: f(x) = u16(u16(x*x) + (x >> 2)) ---
|
|
|
|
#define IMMO_FIXUP 0x2001
|
|
|
|
static uint16_t immo_f(uint16_t x)
|
|
{
|
|
uint16_t sq = (uint16_t)(x * x);
|
|
return (uint16_t)(sq + (x >> 2));
|
|
}
|
|
|
|
// --- Advance: compute next tx and prediction ---
|
|
|
|
uint16_t FIEONA_advance()
|
|
{
|
|
uint16_t rx = _rx_fresh ? _rx_received : _immo_next2;
|
|
_rx_fresh = 0;
|
|
|
|
// Phase 1: tx = f(rx), fixup if < 2
|
|
uint16_t tx = immo_f(rx);
|
|
if (tx < 2) tx = IMMO_FIXUP;
|
|
|
|
// Phase 2: next2 = f(tx), blacklist check
|
|
uint16_t next2 = immo_f(tx);
|
|
if (next2 < 2 || next2 == 0x054B || next2 == 0x4656 || next2 == 0x88FC)
|
|
next2 = IMMO_FIXUP;
|
|
|
|
FIEONA_FIRSTWORD = tx;
|
|
FIEONA_SECONDWORD = tx & 0xFF00;
|
|
_immo_next2 = next2;
|
|
return tx;
|
|
}
|
|
|
|
// --- RX Handler ---
|
|
|
|
static uint16_t last_send4_tick = 0; // last tick for this timeout
|
|
extern uint8_t count;
|
|
|
|
void Fieona_SEND4_Handler(
|
|
const struct CanMessageDef *msg,
|
|
const uint8_t in_data[8],
|
|
CanTxFn tx
|
|
)
|
|
{
|
|
if(count > 1){
|
|
last_send4_tick = TIM16->CNT;
|
|
}
|
|
// Extract first word (little endian)
|
|
uint16_t rx_word = (uint16_t)in_data[0] | ((uint16_t)in_data[1] << 8);
|
|
|
|
// Desync check (firmware validate_rx_word_and_aux behavior)
|
|
if (rx_word != _immo_next2) {
|
|
if (rx_word != IMMO_FIXUP) {
|
|
rx_word = 0;
|
|
}
|
|
}
|
|
|
|
_rx_received = rx_word;
|
|
_rx_fresh = 1;
|
|
}
|
|
|
|
|
|
|
|
uint16_t FIEONA_TRANS0 = 0x0507;
|
|
uint16_t FIEONA_TRANS1 = 0x381C;
|
|
uint16_t FIEONA_TRANS2 = 0xFC1C;
|
|
uint16_t FIEONA_TRANS3 = 0x375B;//0x71F5
|
|
|
|
uint8_t eps_fieona_override = 0;
|
|
|
|
#define FIEONA_UNLOCK_MS 600000
|
|
uint32_t fieona_unlock_end = 0;
|
|
uint8_t fieona_unlocked = 0;
|
|
void Fieona_SEND3_Handler(const uint8_t in_data[8]){
|
|
uint16_t rx_word = (uint16_t)in_data[0] | ((uint16_t)in_data[1] << 8);
|
|
|
|
if(fieona_unlocked){ //si recibo un 700, despues de que este bloqueada, y no es 23 o 24, no contestamos
|
|
if(rx_word == 0x0023){
|
|
FIEONA_TRANS0 = 0xAA63;
|
|
FIEONA_TRANS2 = 0x0001;
|
|
FIEONA_TRANS2 = 0x2100;
|
|
eps_fieona_override = 1;
|
|
}else if(rx_word == 0x0024){
|
|
FIEONA_TRANS0 = 0x0164;
|
|
FIEONA_TRANS2 = 0x0001;
|
|
FIEONA_TRANS2 = 0;
|
|
eps_fieona_override = 1;
|
|
}else{
|
|
Timeout_StopByIndex(18);
|
|
}
|
|
Timeout_ResetByIndex(1, TIM16->CNT);
|
|
|
|
}else{ //si no esta bloqueado
|
|
if(rx_word == 0x0024){ //y es 24 -> envio como desinmovilizando pero sin 2100
|
|
FIEONA_TRANS0 = 0x0164;
|
|
FIEONA_TRANS1 = 0x0001;
|
|
FIEONA_TRANS2 = 0;
|
|
|
|
if(!fieona_unlocked){
|
|
fieona_unlock_end = 0; //si estaba desbloqueando fuck you (creo que deberia ir a si...
|
|
}
|
|
eps_fieona_override = 1;
|
|
|
|
}else if(rx_word == 0x00B2){ //y es b2 -> envio desinmovilizando con 2100
|
|
FIEONA_TRANS0 = 0x0164;
|
|
FIEONA_TRANS1 = 0x0001;
|
|
FIEONA_TRANS2 = 0x2100;
|
|
eps_fieona_override = 1;
|
|
|
|
if(!fieona_unlock_end){ //si no habia empezado a desinmovilizar empiezo
|
|
Timeout_StartIfStopped(1, TIM16->CNT);
|
|
|
|
fieona_unlock_end = HAL_GetTick() + FIEONA_UNLOCK_MS; //roll over is 50 days with 32bit hal tick
|
|
}else{ //si habia empezado, chekiamos si ya acabo.
|
|
if(HAL_GetTick() > fieona_unlock_end){
|
|
fieona_unlocked = 1;
|
|
fieona_unlock_end = 0;
|
|
}
|
|
}
|
|
}else{ //si envio cualquier otra cosa dejo de desinmovilizar y no overrideo na.
|
|
//fieona_unlocked = 0; //aqui creo que deberia volver a bloquearla, pero no voy a tocar na por ahora.
|
|
fieona_unlock_end = 0;
|
|
eps_fieona_override = 0;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
void Fieona_SEND3_set_init(uint8_t i){
|
|
if(i){
|
|
FIEONA_TRANS0 = 0x0507;
|
|
}else{
|
|
FIEONA_TRANS0 = 0x7F81;
|
|
}
|
|
FIEONA_TRANS1 = 0x381C;
|
|
FIEONA_TRANS2 = 0xFC1C;
|
|
FIEONA_TRANS3 = 0x71F5;
|
|
}
|
|
|
|
void FIEONA_Poll(){
|
|
if(last_send4_tick){
|
|
uint16_t newtick = last_send4_tick - 250;
|
|
Timeout_ResetByIndex(20, newtick); // deberia mejorarlo bastante esto, en vez de
|
|
last_send4_tick = 0;
|
|
}
|
|
}
|