Firmware/hpsg5-controller_v2-stm32g4
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
40bda00efa14058f6e2a1c1dd8bc72023ee32623
hpsg5-controller_v2-stm32g4/Core/Kline_Libs/kl_app.c

581 lines
19 KiB
C
Raw Blame History

/*
* kl_app.c
* Application layer: command dispatch, handlers, generic stream sender.
*
* All handlers are non-blocking. They prepare response payloads and
* start TX via the transport layer. KL_App_Service() drives the
* multi-step response state machines.
*/
#include "kl_app.h"
#include "kl_transport.h"
#include "kl_session.h"
#include "psg_prop.h"
#include "ee_manager.h"
#include <string.h>
/* s_dfi_code: shared with ee_manager via extern in ee_manager.h */
int8_t s_dfi_code = 0;
/* ================================================================
* Application context
* ================================================================ */
static struct {
KL_AppState state;
/* Single-response payload */
uint8_t resp_payload[KL_MAX_PACKET_PAYLOAD];
uint8_t resp_len;
/* Stream sender */
uint8_t stream_buf[KL_STREAM_BUF_SIZE];
uint8_t stream_len;
uint8_t stream_offset;
uint8_t stream_cmd; /* 0xF6 for ASCII, 0xFC for DTC */
uint8_t stream_chunk_size; /* 13 for ASCII, 15 for DTC */
uint8_t stream_cur_chunk; /* Current chunk size being sent */
/* EEPROM/ROM unlock flags */
uint8_t eeprom_unlocked_dfi;
uint8_t eeprom_unlocked_dfi_write;
uint8_t rom_unlocked_cust;
} app;
/* ================================================================
* DTC storage
* ================================================================ */
static KL_FaultCode s_dtc_list[KL_DTC_TOTAL_RECORDS];
static size_t s_dtc_count = 0;
/* Default test DTCs */
/*static const KL_FaultCode s_default_dtcs[] = {
{ 0x5B, 0x20, 0x55 },
{ 0x50, 0x20, 0x03 },
{ 0x56, 0x20, 0x56 },
{ 0x5E, 0x30, 0x60 },
};*/
static const KL_FaultCode s_default_dtcs[] = {
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
{ 0x00, 0xFF, 0xFF },
};
/* ================================================================
* Password / login bodies (from old kline.c)
* ================================================================ */
static const uint8_t kPwdBody_DFI[] = { 0x00, 0x03, 0xFF, 0xFF };
static const uint8_t kLoginBody_DFI[] = { 0x00, 0x00, 0x00, 0xBF, 0x30, 0x35, 0x30, 0x30, 0x30, 0x31, 0x1C, 0x09, 0x04 };
static const uint8_t kPwdBody_DFI_WRITE[] = { 0x00, 0x03, 0x2F, 0xFF, 'K','H','T','C','A','8','G','0','E' };
static const uint8_t kPwdBody_DFI_WRITE_815[] = { 0x00, 0x03, 0x2F, 0xFF, '0','5','0','0','0','1',0x1C,0x09,0x04 };
static const uint8_t kPwdBody_CUST[] = { 0x00, 0x00, 0x82, 0x33 };
static const uint8_t kLoginBody_CUST[] = { 0x00, 0x00, 0x9F, 0xFF, 0x30, 0x35, 0x30, 0x30, 0x30, 0x31, 0x1C, 0x09, 0x04 };
static const uint8_t kPwdBody_OEM_1[] = { 0x00, 0x02, 0x2E, 0x10 };
static const uint8_t kLoginBody_OEM_1[] = { 0x9F, 0xD0, 0x9F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
static const uint8_t kReadIdentAddrBody[] = { 0x02, 0x00, 0xC6 };
static const uint8_t kReadFahrSoftwareAddress[] = { 0x02, 0x00, 0xA4 };
static const uint8_t kReadStart55Address[] = { 0x01, 0x00, 0xAE };
static const uint8_t kReadCompensationValue[] = { 0x02, 0x02, 0xFE };
static const uint8_t kActivateSyncOut[] = { 0x88, 0x01, 0x04, 0x06, 0x01 };
/* ================================================================
* Init / Reset
* ================================================================ */
void KL_App_Init(void)
{
memset(&app, 0, sizeof(app));
app.state = KL_APP_IDLE;
KL_App_SetDTCList(s_default_dtcs, sizeof(s_default_dtcs) / sizeof(s_default_dtcs[0]));
}
void KL_App_ResetLocks(void)
{
app.eeprom_unlocked_dfi = 0;
app.eeprom_unlocked_dfi_write = 0;
app.rom_unlocked_cust = 0;
}
void KL_App_Reset(void)
{
app.state = KL_APP_IDLE;
app.eeprom_unlocked_dfi = 0;
app.eeprom_unlocked_dfi_write = 0;
app.rom_unlocked_cust = 0;
}
int KL_App_Busy(void)
{
return (app.state != KL_APP_IDLE && app.state != KL_APP_DONE) ? 1 : 0;
}
int KL_App_Done(void)
{
return (app.state == KL_APP_DONE || app.state == KL_APP_IDLE) ? 1 : 0;
}
/* ================================================================
* DTC management
* ================================================================ */
void KL_App_SetDTCList(const KL_FaultCode *list, size_t count)
{
if (!list) { s_dtc_count = 0; return; }
if (count > KL_DTC_TOTAL_RECORDS) count = KL_DTC_TOTAL_RECORDS;
for (size_t i = 0; i < count; ++i) s_dtc_list[i] = list[i];
s_dtc_count = count;
}
/* ================================================================
* DTC stream builder
* ================================================================ */
static size_t build_dtc_stream(uint8_t *out)
{
size_t w = 0;
for (size_t i = 0; i < KL_DTC_TOTAL_RECORDS; ++i) {
if (i < s_dtc_count) {
out[w++] = s_dtc_list[i].dtc;
out[w++] = s_dtc_list[i].status;
out[w++] = s_dtc_list[i].extra;
} else {
out[w++] = 0x00;
out[w++] = 0xFF;
out[w++] = 0xFF;
}
/* Pad to 8-byte record */
for (int j = 0; j < 5; j++) out[w++] = 0xFF;
}
return w; /* 64 */
}
/* ================================================================
* Ident stream builder (for ReadIdent command)
* ================================================================ */
static size_t app_build_ident(uint8_t *out, size_t maxlen)
{
size_t w = 0;
const char *strs[] = {
PSG_KUNDENNUMMER_STR, PSG_DATENSATZ_STR,
PSG_SOFTWARE_VER_STR, PSG_SOFTWARE_VER2_STR,
PSG_STEUERGERAET_STR, PSG_UNKNOWN_STR
};
const size_t lens[] = { 12, 10, 10, 10, 10, 6 };
for (int s = 0; s < 6; s++) {
for (size_t i = 0; i < lens[s] && strs[s][i] != '\0' && w < maxlen; i++) {
out[w++] = (uint8_t)strs[s][i];
}
}
return w;
}
/* ================================================================
* Helper: start a single-response send
* ================================================================ */
static void start_response(const uint8_t *payload, uint8_t len)
{
memcpy(app.resp_payload, payload, len);
app.resp_len = len;
KL_Transport_SendPacket(app.resp_payload, app.resp_len);
app.state = KL_APP_RESP_WAIT_TX;
}
/* ================================================================
* Helper: start a stream send
* ================================================================ */
static void start_stream(uint8_t cmd, uint8_t chunk_size, const uint8_t *data, uint8_t len)
{
memcpy(app.stream_buf, data, len);
app.stream_len = len;
app.stream_offset = 0;
app.stream_cmd = cmd;
app.stream_chunk_size = chunk_size;
app.state = KL_APP_STREAM_SEND;
}
/* ================================================================
* Command handlers
* ================================================================ */
static void handle_ack(void)
{
KL_Transport_SendAck();
app.state = KL_APP_SIMPLE_WAIT_TX;
}
static void handle_read_ident(void)
{
uint8_t buf[KL_STREAM_BUF_SIZE];
size_t n = app_build_ident(buf, sizeof(buf));
start_stream((uint8_t)KL_CMD_AsciiData, KL_ASCII_CHUNK_SIZE, buf, (uint8_t)n);
}
static void handle_fault_codes_read(void)
{
uint8_t buf[KL_DTC_TOTAL_RECORDS * KL_DTC_RECORD_SIZE];
size_t n = build_dtc_stream(buf);
start_stream((uint8_t)KL_CMD_FaultCodesResponse, KL_DTC_CHUNK_SIZE, buf, (uint8_t)n);
}
static void handle_read_eeprom(const KL_Packet *cmd)
{
if (cmd->data_len < 3U) { KL_Transport_SendNak(); app.state = KL_APP_SIMPLE_WAIT_TX; return; }
uint8_t len_req = cmd->data[0];
uint16_t addr = ((uint16_t)cmd->data[1] << 8) | cmd->data[2];
if (addr == 0x0044 && !app.eeprom_unlocked_dfi) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
uint8_t data[16] = {0};
if (!PSG_EEPROM_GetData(addr, len_req, data)) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
uint8_t payload[1 + 16];
payload[0] = (uint8_t)KL_CMD_ReadEepromResponse;
memcpy(&payload[1], data, len_req);
start_response(payload, (uint8_t)(1U + len_req));
}
static void handle_read_rom_eeprom(const KL_Packet *cmd)
{
if (cmd->data_len < 3U) { KL_Transport_SendNak(); app.state = KL_APP_SIMPLE_WAIT_TX; return; }
uint8_t len_req = cmd->data[0];
uint16_t addr = ((uint16_t)cmd->data[1] << 8) | cmd->data[2];
if ((addr == 0x9FFE || addr == (uint16_t)(PSG_CUST_CHANGE_ADDR + 3U)) && !app.rom_unlocked_cust) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
uint8_t data[10];
if (!PSG_ROM_GetData(addr, len_req, data)) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
uint8_t payload[1 + 10];
payload[0] = (uint8_t)KL_CMD_ReadRomResponse;
memcpy(&payload[1], data, len_req);
start_response(payload, (uint8_t)(1U + len_req));
}
static void handle_login_eeprom(const KL_Packet *cmd)
{
const uint8_t *body = cmd->data;
uint8_t body_len = cmd->data_len;
const uint8_t *login_body = NULL;
size_t login_len = 0;
if (body_len == sizeof(kPwdBody_DFI) && memcmp(body, kPwdBody_DFI, sizeof(kPwdBody_DFI)) == 0) {
app.eeprom_unlocked_dfi = 1;
login_body = kLoginBody_DFI;
login_len = sizeof(kLoginBody_DFI);
} else if (body_len == sizeof(kPwdBody_CUST) && memcmp(body, kPwdBody_CUST, sizeof(kPwdBody_CUST)) == 0) {
app.rom_unlocked_cust = 1;
login_body = kLoginBody_CUST;
login_len = sizeof(kLoginBody_CUST);
} else if (body_len == sizeof(kPwdBody_OEM_1) && memcmp(body, kPwdBody_OEM_1, sizeof(kPwdBody_OEM_1)) == 0) {
login_body = kLoginBody_OEM_1;
login_len = sizeof(kLoginBody_OEM_1);
} else if (body_len == sizeof(kPwdBody_DFI_WRITE) && memcmp(body, kPwdBody_DFI_WRITE, sizeof(kPwdBody_DFI_WRITE)) == 0) {
app.eeprom_unlocked_dfi_write = 1;
login_body = kLoginBody_DFI;
login_len = sizeof(kLoginBody_DFI);
}else if (body_len == sizeof(kPwdBody_DFI_WRITE_815) && memcmp(body, kPwdBody_DFI_WRITE_815, sizeof(kPwdBody_DFI_WRITE_815)) == 0) {
app.eeprom_unlocked_dfi_write = 1;
login_body = kLoginBody_DFI;
login_len = sizeof(kLoginBody_DFI);
} else {
/* Wrong password -- clear all locks */
app.eeprom_unlocked_dfi = 0;
app.eeprom_unlocked_dfi_write = 0;
app.rom_unlocked_cust = 0;
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
uint8_t payload[1 + 16];
payload[0] = (uint8_t)KL_CMD_EepromLoginResponse;
memcpy(&payload[1], login_body, login_len);
start_response(payload, (uint8_t)(1U + login_len));
}
static void handle_write_eeprom(const KL_Packet *cmd)
{
if (cmd->data_len < 5U) { KL_Transport_SendNak(); app.state = KL_APP_SIMPLE_WAIT_TX; return; }
uint8_t len_req = cmd->data[0];
uint16_t addr = ((uint16_t)cmd->data[1] << 8) | cmd->data[2];
uint8_t value = cmd->data[3];
uint8_t csum = cmd->data[4];
if (!app.eeprom_unlocked_dfi_write) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
if (addr != 0x0044 || len_req != 0x02) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
if ((uint8_t)(value + csum) != 0) {
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
s_dfi_code = (int8_t)value;
memWrite = 1;
uint8_t payload[5];
payload[0] = (uint8_t)KL_CMD_WriteEepromResponse;
payload[1] = (uint8_t)len_req;
payload[2] = (uint8_t)((addr >> 8) & 0xFF);
payload[3] = (uint8_t)(addr & 0xFF);
payload[4] = 0xFF;
start_response(payload, 5);
}
uint8_t SYNC_PULSE_OUT;
static void handle_read_ram(const KL_Packet *cmd)
{
if (cmd->data_len >= sizeof(kReadIdentAddrBody) &&
memcmp(cmd->data, kReadIdentAddrBody, sizeof(kReadIdentAddrBody)) == 0) {
uint8_t payload[3];
payload[0] = 0xFE;
payload[1] = (uint8_t)(PSG_IDENT_ADDR & 0xFF);
payload[2] = (uint8_t)((PSG_IDENT_ADDR >> 8) & 0xFF);
start_response(payload, 3);
return;
}
if (cmd->data_len >= sizeof(kReadFahrSoftwareAddress) &&
memcmp(cmd->data, kReadFahrSoftwareAddress, sizeof(kReadFahrSoftwareAddress)) == 0) {
uint8_t payload[3];
payload[0] = 0xFE;
payload[1] = (uint8_t)(PSG_FAHRSOFTWARE_ADDR & 0xFF);
payload[2] = (uint8_t)((PSG_FAHRSOFTWARE_ADDR >> 8) & 0xFF);
start_response(payload, 3);
return;
}
if (cmd->data_len >= sizeof(kReadStart55Address) &&
memcmp(cmd->data, kReadStart55Address, sizeof(kReadStart55Address)) == 0) {
uint8_t payload[2];
payload[0] = 0xFE;
payload[1] = (uint8_t)(PSG_START55_ADDR);
start_response(payload, 2);
return;
}
if (cmd->data_len >= sizeof(kReadCompensationValue) &&
memcmp(cmd->data, kReadCompensationValue, sizeof(kReadCompensationValue)) == 0) {
uint8_t payload[3];
payload[0] = 0xFE;
payload[1] = (uint8_t)(PSG_CALVALUE_ADDR & 0xFF);
payload[2] = (uint8_t)((PSG_CALVALUE_ADDR >> 8) & 0xFF);
start_response(payload, 2);
return;
}
/*else{//fallback
uint8_t payload[cmd->data_len-1];
payload[0] = 0xFE;
//payload[1] = (uint8_t)(PSG_START55_ADDR);
//fill with
start_response(payload, 2);
}*/
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
}
static void handle_write_ram(const KL_Packet *cmd)
{
if (cmd->data_len >= sizeof(kActivateSyncOut) &&
memcmp(cmd->data, kActivateSyncOut, sizeof(kActivateSyncOut)) == 0) {
//SYNC_PULSE_OUT = 1;
SYNC_Pulse_RequestSyncout(1);
KL_Transport_SendAck();
app.state = KL_APP_SIMPLE_WAIT_TX;
return;
}
//KL_Transport_SendNak();
KL_Transport_SendAck();
app.state = KL_APP_SIMPLE_WAIT_TX;
}
static void handle_end(void)
{
KL_Transport_SendAck();
app.state = KL_APP_SIMPLE_WAIT_TX;
/* Session layer will check end_requested after we're done */
KL_Session_RequestEnd();
}
/* ================================================================
* Command dispatch
* ================================================================ */
void KL_App_Dispatch(const KL_Packet *cmd)
{
app.state = KL_APP_IDLE;
KL_Transport_TxReset();
switch (cmd->command) {
case KL_CMD_ACK:
handle_ack();
break;
case KL_CMD_ReadIdent:
handle_read_ident();
break;
case KL_CMD_FaultCodesRead:
handle_fault_codes_read();
break;
case KL_CMD_ReadEeprom:
handle_read_eeprom(cmd);
break;
case KL_CMD_ReadRomEeprom:
handle_read_rom_eeprom(cmd);
break;
case KL_CMD_LoginEeprom:
handle_login_eeprom(cmd);
break;
case KL_CMD_WriteEeprom:
handle_write_eeprom(cmd);
break;
case KL_CMD_ReadRam:
handle_read_ram(cmd);
break;
case KL_CMD_WriteRam:
handle_write_ram(cmd);
break;
case KL_CMD_End:
handle_end();
break;
default:
KL_Transport_SendNak();
app.state = KL_APP_SIMPLE_WAIT_TX;
break;
}
}
/* ================================================================
* Service: drive response state machines
* ================================================================ */
void KL_App_Service(void)
{
switch (app.state) {
case KL_APP_IDLE:
case KL_APP_DONE:
break;
/* ---- Simple send (ACK/NAK, no follow-up ACK exchange) ---- */
case KL_APP_SIMPLE_WAIT_TX:
if (KL_Transport_TxDone()) {
KL_Transport_TxReset();
app.state = KL_APP_DONE;
} else if (KL_Transport_TxError()) {
KL_Transport_TxReset();
app.state = KL_APP_DONE;
}
break;
/* ---- Single response: send and return to ready ---- */
case KL_APP_RESP_WAIT_TX:
if (KL_Transport_TxDone()) {
KL_Transport_TxReset();
KL_Session_Kick();
app.state = KL_APP_DONE;
} else if (KL_Transport_TxError()) {
KL_Transport_TxReset();
app.state = KL_APP_DONE;
}
break;
/* ---- Stream: send chunks -> wait ACK each -> final ACK ---- */
case KL_APP_STREAM_SEND:
{
uint8_t remaining = app.stream_len - app.stream_offset;
app.stream_cur_chunk = (remaining > app.stream_chunk_size)
? app.stream_chunk_size : remaining;
uint8_t payload[1 + KL_DTC_CHUNK_SIZE]; /* max chunk size */
payload[0] = app.stream_cmd;
memcpy(&payload[1], &app.stream_buf[app.stream_offset], app.stream_cur_chunk);
if (KL_Transport_SendPacket(payload, (uint8_t)(1 + app.stream_cur_chunk))) {
app.state = KL_APP_STREAM_WAIT_TX;
}
break;
}
case KL_APP_STREAM_WAIT_TX:
if (KL_Transport_TxDone()) {
KL_Transport_TxReset();
app.stream_offset += app.stream_cur_chunk;
if (app.stream_offset >= app.stream_len) {
/* Last chunk sent -- return to ready state.
* No ACK is expected after the final chunk.
* The master's next packet (keepalive ACK or command)
* will be handled normally by KL_App_Dispatch. */
KL_Session_Kick();
app.state = KL_APP_DONE;
} else {
/* Intermediate chunk: wait for ACK before sending next */
KL_Transport_StartReceive(400);
app.state = KL_APP_STREAM_WAIT_ACK;
}
} else if (KL_Transport_TxError()) {
KL_Transport_TxReset();
app.state = KL_APP_DONE;
}
break;
case KL_APP_STREAM_WAIT_ACK:
if (KL_Transport_RxDone()) {
const KL_Packet *ack = KL_Transport_GetRxPacket();
KL_Transport_RxReset();
if (ack->command == (uint8_t)KL_CMD_ACK) {
KL_Session_Kick();
app.state = KL_APP_STREAM_SEND;
} else {
app.state = KL_APP_DONE;
}
} else if (KL_Transport_RxError()) {
KL_Transport_RxReset();
app.state = KL_APP_DONE;
}
break;
case KL_APP_STREAM_FINAL_WAIT:
/* No longer used, but kept for safety */
if (KL_Transport_TxDone()) {
KL_Transport_TxReset();
KL_Session_Kick();
app.state = KL_APP_DONE;
} else if (KL_Transport_TxError()) {
KL_Transport_TxReset();
app.state = KL_APP_DONE;
}
break;
default:
app.state = KL_APP_DONE;
break;
}
}
Reference in New Issue View Git Blame Copy Permalink