508 lines
15 KiB
C
508 lines
15 KiB
C
//
|
||
// Created by cfif on 16.03.2024.
|
||
//
|
||
#include <SystemDelayInterface.h>
|
||
#include "CanSerialPortFrameTP.h"
|
||
#include "memory.h"
|
||
#include "AsciiStringAssmeblingUtils.h"
|
||
#include "SystemMutexCmsis.h"
|
||
#include "CanPorts.h"
|
||
#include <stdio.h>
|
||
|
||
#define LOG_SIGN "CAN_TP"
|
||
#define LOGGER env->logger
|
||
|
||
|
||
static char strPrintfDebug_TP[80];
|
||
|
||
static void PrintfDebug(uint8_t *data, uint8_t dlc) {
|
||
|
||
switch (dlc) {
|
||
|
||
case 0:
|
||
sprintf(strPrintfDebug_TP, " ");
|
||
break;
|
||
case 1:
|
||
sprintf(strPrintfDebug_TP, "%02X", data[0]);
|
||
break;
|
||
case 2:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X", data[0], data[1]);
|
||
break;
|
||
case 3:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X", data[0], data[1], data[2]);
|
||
break;
|
||
case 4:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3]);
|
||
break;
|
||
case 5:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3],
|
||
data[4]);
|
||
break;
|
||
case 6:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3],
|
||
data[4], data[5]);
|
||
break;
|
||
case 7:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2],
|
||
data[3], data[4],
|
||
data[5], data[6]);
|
||
break;
|
||
case 8:
|
||
sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X ", data[0], data[1],
|
||
data[2], data[3], data[4],
|
||
data[5], data[6], data[7]);
|
||
break;
|
||
default:
|
||
sprintf(strPrintfDebug_TP, " ");
|
||
}
|
||
|
||
}
|
||
|
||
|
||
char *sendLogCanTpHex(tCanSerialPortFrameTp *env, uint8_t *data, size_t size) {
|
||
memset(env->hexString, 0, sizeof(env->hexString));
|
||
size_t len = 0;
|
||
|
||
|
||
uint8_t full = size / 8;
|
||
uint8_t tail = size % 8;
|
||
|
||
for (uint8_t i = 0; i < full; ++i) {
|
||
PrintfDebug(&data[i * 8], 8);
|
||
|
||
if ((len + strlen(strPrintfDebug_TP)) < LEN_DEBUG_TP_BUFF) {
|
||
memcpy(&env->hexString[len], strPrintfDebug_TP, strlen(strPrintfDebug_TP));
|
||
len += strlen(strPrintfDebug_TP);
|
||
} else {
|
||
return env->hexString;
|
||
}
|
||
|
||
}
|
||
|
||
if (tail > 0) {
|
||
PrintfDebug(&data[full * 8], tail);
|
||
|
||
if ((len + strlen(strPrintfDebug_TP)) < LEN_DEBUG_TP_BUFF) {
|
||
memcpy(&env->hexString[len], strPrintfDebug_TP, strlen(strPrintfDebug_TP));
|
||
len += strlen(strPrintfDebug_TP);
|
||
} else {
|
||
return env->hexString;
|
||
}
|
||
|
||
}
|
||
|
||
return env->hexString;
|
||
}
|
||
|
||
uint16_t sendFlowFrame(tCanSerialPortFrameTp *env, uint32_t id, uint32_t timeout) {
|
||
uint8_t dataTpFrame[8];
|
||
memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));
|
||
|
||
eTpFrameFC *frame = (eTpFrameFC *) dataTpFrame;
|
||
frame->typeFrame = TP_TYPE_FRAME_FC;
|
||
frame->status = 0;
|
||
frame->blockSize = 0;
|
||
frame->timeST = 0;
|
||
|
||
uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
|
||
PROTOCOL_CAN_TYPE_UDS, timeout);
|
||
|
||
return sent;
|
||
}
|
||
|
||
|
||
uint16_t sendSingleFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
|
||
uint8_t dataTpFrame[8];
|
||
memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));
|
||
|
||
eTpFrameSF *frame = (eTpFrameSF *) dataTpFrame;
|
||
frame->typeFrame = TP_TYPE_FRAME_SF;
|
||
frame->dl = size;
|
||
|
||
memcpy(&frame->data[0], data, size);
|
||
|
||
uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
|
||
PROTOCOL_CAN_TYPE_UDS, timeout);
|
||
|
||
return sent;
|
||
}
|
||
|
||
uint16_t sendConsecutiveFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint8_t sn,
|
||
uint32_t timeout) {
|
||
uint8_t dataTpFrame[8];
|
||
memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));
|
||
|
||
eTpFrameCF *frame = (eTpFrameCF *) dataTpFrame;
|
||
frame->typeFrame = TP_TYPE_FRAME_CF;
|
||
frame->sn = sn;
|
||
|
||
memcpy(&frame->data[0], data, size);
|
||
|
||
|
||
uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
|
||
PROTOCOL_CAN_TYPE_UDS, timeout);
|
||
|
||
|
||
return sent;
|
||
|
||
}
|
||
|
||
bool waitFlowControl(tCanSerialPortFrameTp *env) {
|
||
env->isFlowControl = true;
|
||
|
||
uint32_t timeEnd = SystemGetMs() + WAIT_FC_FRAME_TIMEOUT;
|
||
|
||
while (timeEnd > SystemGetMs()) {
|
||
if (!env->isFlowControl) {
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (env->isFlowControl) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Flow control frame timeout");
|
||
return false;
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool sendFirstFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
|
||
uint8_t dataTpFrame[8];
|
||
uint16_t pDataLen = 0;
|
||
|
||
memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));
|
||
|
||
eTpFrameFF *frame = (eTpFrameFF *) dataTpFrame;
|
||
frame->typeFrame = TP_TYPE_FRAME_FF;
|
||
frame->dlL = size;
|
||
frame->dlM = (size >> 8);
|
||
|
||
memcpy(&frame->data[0], &data[pDataLen], 6);
|
||
pDataLen += 6;
|
||
|
||
uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
|
||
PROTOCOL_CAN_TYPE_UDS, timeout);
|
||
|
||
bool result = waitFlowControl(env);
|
||
if (!result)
|
||
return false;
|
||
|
||
uint16_t countPacketFull = (size - 6) / 7;
|
||
uint16_t sizePacketTail = (size - 6) % 7;
|
||
|
||
uint8_t sn = 1;
|
||
|
||
for (uint16_t i = 0; i < countPacketFull; ++i) {
|
||
|
||
sendConsecutiveFrame(env, &data[pDataLen], 7, id, sn, timeout);
|
||
pDataLen += 7;
|
||
|
||
++sn;
|
||
if (sn == 15)
|
||
sn = 1;
|
||
}
|
||
|
||
if (sizePacketTail) {
|
||
sendConsecutiveFrame(env, &data[pDataLen], sizePacketTail, id, sn, timeout);
|
||
pDataLen += sizePacketTail;
|
||
}
|
||
|
||
return true;
|
||
|
||
}
|
||
|
||
bool
|
||
CanSerialPortFrameTpTransmit(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
|
||
bool result;
|
||
|
||
// Single Frame SF
|
||
if (size <= 7) {
|
||
result = sendSingleFrame(env, data, size, id, timeout);
|
||
} else {
|
||
result = sendFirstFrame(env, data, size, id, timeout);
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
eTpResult vCanSerialPortFrameTpReceive(tCanSerialPortFrameTp *env, uint32_t timeout) {
|
||
can_rx_message_type canFrame;
|
||
|
||
if ((env->timeoutFF > 0) && (env->timeoutFF < SystemGetMs())) {
|
||
env->timeoutFF = 0;
|
||
env->dataReceivedConsecutiveFrameLen = 0;
|
||
env->dataBufLen = 0;
|
||
env->dataBufWorkLen = 0;
|
||
|
||
return TP_ERROR_FF_ERROR_TIMEOUT;
|
||
}
|
||
|
||
uint16_t recv = 0;
|
||
|
||
recv = env->ioCanFrame->receive(env->ioCanFrame->env, PROTOCOL_CAN_UDS, (uint8_t *) &canFrame, 1, timeout);
|
||
|
||
if (recv == 0)
|
||
return TP_STANDBY;
|
||
|
||
bool isTesterPresent = env->ReceivedCan_func(env->callback_argCan, &canFrame);
|
||
|
||
if (isTesterPresent) {
|
||
return TP_STANDBY;
|
||
}
|
||
|
||
bool isFilterFind = false;
|
||
uint32_t reqId = 0;
|
||
for (uint8_t i = 0; i < env->filterIdCount; ++i) {
|
||
if (canFrame.standard_id == env->filterReqId[i]) {
|
||
|
||
reqId = env->filterRespId[i];
|
||
|
||
isFilterFind = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!isFilterFind) {
|
||
return TP_ERROR_WRONG_ADDRESS;
|
||
}
|
||
|
||
|
||
if ((recv > 0) && (canFrame.dlc == 8)) {
|
||
|
||
// Flow control Frame – FC – фрейм управления потоком
|
||
if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_FC) {
|
||
eTpFrameFC *frame = (eTpFrameFC *) canFrame.data;
|
||
|
||
if (frame->status == 0)
|
||
return TP_RECEIVED_FC_DATA;
|
||
|
||
return TP_ERROR_FC_ERROR;
|
||
}
|
||
|
||
// Single Frame SF – Однократный фрейм
|
||
if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_SF) {
|
||
eTpFrameSF *frame = (eTpFrameSF *) canFrame.data;
|
||
|
||
if (frame->dl < env->dataBufMaxSize) {
|
||
//memcpy(env->dataBuf, &canFrame.data[1], frame->dl);
|
||
env->dataBufLen = frame->dl;
|
||
|
||
memcpy(env->dataBuf->data, &canFrame.data[1], frame->dl);
|
||
env->dataBuf->len = env->dataBufLen;
|
||
env->dataBuf->adrCan = canFrame.standard_id;
|
||
|
||
env->dataBufWorkLen = 0;
|
||
return TP_RECEIVED_DATA;
|
||
} else {
|
||
env->dataBufWorkLen = 0;
|
||
return TP_ERROR_FS_ERROR_SIZE;
|
||
}
|
||
}
|
||
|
||
// First Frame FF – Первый фрейм из серии фреймов
|
||
if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_FF) {
|
||
if (env->dataReceivedConsecutiveFrameLen) {
|
||
return TP_ERROR_FF_ERROR;
|
||
}
|
||
|
||
eTpFrameFF *frame = (eTpFrameFF *) canFrame.data;
|
||
|
||
if (env->dataBufWorkLen + (frame->dlL | (frame->dlM << 8)) > env->dataBufMaxSize) {
|
||
return TP_ERROR_FF_ERROR_SIZE;
|
||
}
|
||
|
||
uint16_t dll = frame->dlL | (frame->dlM << 8);
|
||
if (dll > 100) {
|
||
asm("nop");
|
||
}
|
||
|
||
env->timeoutFF = SystemGetMs() + WAIT_FF_FRAME_TIMEOUT;
|
||
|
||
env->sn = 1;
|
||
env->dataBufWorkLen = 0;
|
||
env->dataReceivedConsecutiveFrameLen = (frame->dlL | (frame->dlM << 8));
|
||
|
||
for (uint8_t j = 2; j < 8; ++j) {
|
||
//env->dataBuf[env->dataBufWorkLen] = canFrame.data[j];
|
||
env->dataBuf->data[env->dataBufWorkLen] = canFrame.data[j];
|
||
|
||
++env->dataBufWorkLen;
|
||
if (env->dataBufWorkLen > env->dataBufMaxSize) {
|
||
env->timeoutFF = 0;
|
||
env->dataReceivedConsecutiveFrameLen = 0;
|
||
env->dataBufWorkLen = 0;
|
||
return TP_ERROR_CF_ERROR_SIZE;
|
||
}
|
||
|
||
--env->dataReceivedConsecutiveFrameLen;
|
||
}
|
||
|
||
sendFlowFrame(env, reqId, WAIT_FRAME_WRITE);
|
||
}
|
||
|
||
// Consecutive Frame CF – последующий фрейм.
|
||
if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_CF) {
|
||
|
||
if (env->dataReceivedConsecutiveFrameLen == 0) {
|
||
return TP_ERROR_CF_ERROR;
|
||
}
|
||
|
||
eTpFrameCF *frame = (eTpFrameCF *) canFrame.data;
|
||
|
||
if (frame->sn != env->sn) {
|
||
env->timeoutFF = 0;
|
||
env->dataReceivedConsecutiveFrameLen = 0;
|
||
env->dataBufWorkLen = 0;
|
||
return TP_ERROR_CF_ERROR_SN;
|
||
}
|
||
|
||
++env->sn;
|
||
if (env->sn > 15)
|
||
env->sn = 0;
|
||
|
||
for (uint8_t j = 1; j < 8; ++j) {
|
||
//env->dataBuf[env->dataBufWorkLen] = canFrame.data[j];
|
||
env->dataBuf->data[env->dataBufWorkLen] = canFrame.data[j];
|
||
|
||
++env->dataBufWorkLen;
|
||
if (env->dataBufWorkLen > env->dataBufMaxSize) {
|
||
env->timeoutFF = 0;
|
||
env->dataReceivedConsecutiveFrameLen = 0;
|
||
env->dataBufWorkLen = 0;
|
||
return TP_ERROR_CF_ERROR_SIZE;
|
||
}
|
||
|
||
--env->dataReceivedConsecutiveFrameLen;
|
||
if (env->dataReceivedConsecutiveFrameLen == 0) {
|
||
env->timeoutFF = 0;
|
||
env->dataBufLen = env->dataBufWorkLen;
|
||
|
||
env->dataBuf->len = env->dataBufLen;
|
||
env->dataBuf->adrCan = canFrame.standard_id;
|
||
|
||
env->dataBufWorkLen = 0;
|
||
return TP_RECEIVED_DATA;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
return TP_ERROR_FC_TIMEOUT;
|
||
}
|
||
|
||
|
||
_Noreturn void CanTpProcessing_ListenerTask(tCanSerialPortFrameTp *env) {
|
||
while (1) {
|
||
eTpResult result = vCanSerialPortFrameTpReceive(env, 1000);
|
||
|
||
if (result == TP_RECEIVED_FC_DATA) {
|
||
if (env->isFlowControl) {
|
||
env->isFlowControl = false;
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Flow control frame accepted");
|
||
} else {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: An unexpected Flow Control Frame was received");
|
||
}
|
||
}
|
||
|
||
if (result == TP_RECEIVED_DATA) {
|
||
//sendLogCanTpHex(env, env->dataBuf->data, env->dataBuf->len);
|
||
//LoggerFormatInfo(LOGGER, LOG_SIGN, "Data received: %s", env->hexString)
|
||
|
||
env->receivedTP_func(env->callback_argTp, env->dataBuf);
|
||
|
||
}
|
||
|
||
if (result == TP_ERROR_FS_ERROR_SIZE) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (1)");
|
||
}
|
||
|
||
if (result == TP_ERROR_FC_ERROR) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: An error status was set in the Flow Control Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN,
|
||
"Error: First Frame FF received, but Consecutive Frame reception is not yet complete");
|
||
}
|
||
|
||
|
||
if (result == TP_ERROR_FF_ERROR_SIZE) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (2)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Unexpected Consecutive Frame received");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SIZE) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (3)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SN) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: SN counter mismatch in Consecutive Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
|
||
LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: The Consecutive Frame timed out");
|
||
}
|
||
}
|
||
}
|
||
|
||
void CanSerialPortFrameTp_Start(tCanSerialPortFrameTp *env) {
|
||
ThreadBlock_Start(env->T_can_Listener, env, CanTpProcessing_ListenerTask);
|
||
}
|
||
|
||
void CanSerialPortFrameTpInit(
|
||
tCanSerialPortFrameTp *env,
|
||
|
||
tReceivedCan_func receivedCan_func,
|
||
void *callback_argCan,
|
||
|
||
tReceivedTP_func receivedTP_func,
|
||
void *callback_arg,
|
||
|
||
tSerialPortFrameIO *ioCanFrame,
|
||
tCanTP_data *dataBuf,
|
||
uint16_t dataBufMaxSize,
|
||
tLoggerInterface *logger,
|
||
|
||
uint8_t filterIdCount,
|
||
uint32_t *filterReqId,
|
||
uint32_t *filterRespId,
|
||
uint8_t *filterDirReq
|
||
|
||
) {
|
||
env->ioCanFrame = ioCanFrame;
|
||
|
||
env->dataBuf = dataBuf;
|
||
env->dataBufMaxSize = dataBufMaxSize;
|
||
env->dataBufLen = 0;
|
||
env->dataBufWorkLen = 0;
|
||
env->isFlowControl = false;
|
||
|
||
env->filterIdCount = filterIdCount;
|
||
env->filterReqId = filterReqId;
|
||
env->filterRespId = filterRespId;
|
||
env->filterDirReq = filterDirReq;
|
||
|
||
env->dataReceivedConsecutiveFrameLen = 0;
|
||
|
||
|
||
env->ReceivedCan_func = receivedCan_func;
|
||
env->callback_argCan = callback_argCan;
|
||
|
||
|
||
env->receivedTP_func = receivedTP_func;
|
||
env->callback_argTp = callback_arg;
|
||
|
||
env->timeoutFF = 0;
|
||
|
||
env->logger = logger;
|
||
|
||
InitThreadBlock(env->T_can_Listener, "CanTpUds", osPriorityNormal);
|
||
}
|
||
|
||
|