550 lines
17 KiB
C
550 lines
17 KiB
C
//
|
||
// Created by cfif on 16.03.2024.
|
||
//
|
||
#include <SystemDelayInterface.h>
|
||
#include "CanSerialPortFrameTP.h"
|
||
#include "memory.h"
|
||
#include "AsciiStringAssmeblingUtils.h"
|
||
#include "SystemMutexCmsis.h"
|
||
|
||
|
||
#define LOG_SIGN "CAN"
|
||
#define LOGGER &env->slog->logger
|
||
|
||
_Noreturn void CanTpProcessing_ListenerTask0(tCanSerialPortFrameTp *env);
|
||
|
||
_Noreturn void CanTpProcessing_ListenerTask1(tCanSerialPortFrameTp *env);
|
||
|
||
void CanSerialPortFrameTpInit(
|
||
tCanSerialPortFrameTp *env,
|
||
|
||
uint8_t numBuf,
|
||
|
||
SystemMutexCmsis txAccessQueue,
|
||
|
||
tReceivedCan_func receivedCan_func,
|
||
void *callback_argCan,
|
||
|
||
tReceivedTP_func receivedTP_func,
|
||
void *callback_arg,
|
||
|
||
tSerialPortFrameIO *ioCanFrame,
|
||
tCanTP_data *dataBuf,
|
||
uint16_t dataBufMaxSize,
|
||
tLoggerToSerialPort *slog,
|
||
|
||
uint8_t filterIdCount,
|
||
uint32_t *filterReqId,
|
||
uint32_t *filterRespId,
|
||
uint8_t *filterDirReq
|
||
|
||
) {
|
||
env->ioCanFrame = ioCanFrame;
|
||
|
||
env->numBuf = numBuf;
|
||
|
||
env->txAccessQueue = txAccessQueue;
|
||
|
||
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->slog = slog;
|
||
|
||
InitThreadBlock(env->T_can_Listener0, "CanListener0", osPriorityNormal);
|
||
InitThreadBlock(env->T_can_Listener1, "CanListener1", osPriorityNormal);
|
||
}
|
||
|
||
void CanSerialPortFrameTp_Start(tCanSerialPortFrameTp *env, uint8_t numBuf) {
|
||
if (numBuf == 0)
|
||
ThreadBlock_Start(env->T_can_Listener0, env, CanTpProcessing_ListenerTask0);
|
||
|
||
if (numBuf == 1)
|
||
ThreadBlock_Start(env->T_can_Listener1, env, CanTpProcessing_ListenerTask1);
|
||
}
|
||
|
||
char *sendLogCanHex(tCanSerialPortFrameTp *env, uint8_t *data, size_t size) {
|
||
memset(env->hexString, 0, sizeof(env->hexString));
|
||
size_t len = 0;
|
||
vAsciiStringAddBytesAsHex(env->hexString, &len, data, size);
|
||
|
||
LoggerStrInfo(LOGGER, LOG_SIGN, env->hexString, strlen(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 = 0;
|
||
|
||
if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {
|
||
|
||
CanSerialPortFrameSetId(env->ioCanFrame->env, id);
|
||
|
||
sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, timeout);
|
||
|
||
osMutexRelease(env->txAccessQueue);
|
||
} else {
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendFlowFrame")
|
||
}
|
||
|
||
return sent;
|
||
}
|
||
|
||
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;
|
||
|
||
if (env->numBuf == 0) {
|
||
recv = env->ioCanFrame->receive0(env->ioCanFrame->env, (uint8_t *) &canFrame, 1, timeout);
|
||
}
|
||
|
||
if (env->numBuf == 1) {
|
||
recv = env->ioCanFrame->receive1(env->ioCanFrame->env, (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_ListenerTask0(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");
|
||
} else {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Принят не ожидаемый Flow control Frame");
|
||
}
|
||
}
|
||
|
||
if (result == TP_RECEIVED_DATA) {
|
||
// LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Приняты данные: ");
|
||
// sendLogCanHex(env, env->dataBuf->data, env->dataBuf->len);
|
||
|
||
env->receivedTP_func(env->callback_argTp, env->dataBuf);
|
||
|
||
}
|
||
|
||
if (result == TP_ERROR_FS_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (1)");
|
||
}
|
||
|
||
if (result == TP_ERROR_FC_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Выставлен статус ошибки в Flow control Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN,
|
||
"0: Ошибка. Принят First Frame FF, но еще не завершен прием Consecutive Frame");
|
||
}
|
||
|
||
|
||
if (result == TP_ERROR_FF_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (2)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Принят не ожидаемый Consecutive Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (3)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SN) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Не совпадение счетчика SN в Consecutive Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Истекло время ожидания приема Consecutive Frame");
|
||
}
|
||
}
|
||
}
|
||
|
||
_Noreturn void CanTpProcessing_ListenerTask1(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");
|
||
} else {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Принят не ожидаемый Flow control Frame");
|
||
}
|
||
}
|
||
|
||
if (result == TP_RECEIVED_DATA) {
|
||
// LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Приняты данные: ");
|
||
// sendLogCanHex(env, env->dataBuf->data, env->dataBuf->len);
|
||
|
||
env->receivedTP_func(env->callback_argTp, env->dataBuf);
|
||
|
||
}
|
||
|
||
if (result == TP_ERROR_FS_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (1)");
|
||
}
|
||
|
||
if (result == TP_ERROR_FC_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Выставлен статус ошибки в Flow control Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN,
|
||
"1: Ошибка. Принят First Frame FF, но еще не завершен прием Consecutive Frame");
|
||
}
|
||
|
||
|
||
if (result == TP_ERROR_FF_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (2)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Принят не ожидаемый Consecutive Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SIZE) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (3)");
|
||
}
|
||
|
||
if (result == TP_ERROR_CF_ERROR_SN) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Не совпадение счетчика SN в Consecutive Frame");
|
||
}
|
||
|
||
if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
|
||
LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Истекло время ожидания приема Consecutive Frame");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
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 = 0;
|
||
|
||
if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {
|
||
|
||
CanSerialPortFrameSetId(env->ioCanFrame->env, id);
|
||
|
||
sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, timeout);
|
||
|
||
osMutexRelease(env->txAccessQueue);
|
||
} else {
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendSingleFrame")
|
||
}
|
||
|
||
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 = 0;
|
||
|
||
if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {
|
||
|
||
CanSerialPortFrameSetId(env->ioCanFrame->env, id);
|
||
|
||
sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, timeout);
|
||
|
||
osMutexRelease(env->txAccessQueue);
|
||
} else {
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendConsecutiveFrame")
|
||
}
|
||
|
||
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");
|
||
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 = 0;
|
||
|
||
if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {
|
||
|
||
CanSerialPortFrameSetId(env->ioCanFrame->env, id);
|
||
|
||
sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, timeout);
|
||
|
||
osMutexRelease(env->txAccessQueue);
|
||
} else {
|
||
LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendFirstFrame")
|
||
}
|
||
|
||
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;
|
||
}
|