/**
* This command is needed to make the whole transfer a bit faster
* @note See also handleWriteValueCommand
*/
void handleWriteChunkCommand(ts_channel_s *tsChannel, ts_response_format_e mode, short offset, short count,
void *content) {
tsState.writeChunkCommandCounter++;
scheduleMsg(&tsLogger, "WRITE CHUNK m=%d p=%d o=%d s=%d", mode, currentPageId, offset, count);
if (offset > getTunerStudioPageSize(currentPageId)) {
scheduleMsg(&tsLogger, "ERROR invalid offset %d", offset);
tunerStudioError("ERROR: out of range");
offset = 0;
}
if (count > getTunerStudioPageSize(currentPageId)) {
tunerStudioError("ERROR: unexpected count");
scheduleMsg(&tsLogger, "ERROR unexpected count %d", count);
count = 0;
}
uint8_t * addr = (uint8_t *) (getWorkingPageAddr(currentPageId) + offset);
memcpy(addr, content, count);
yellowMagic(currentPageId, offset, count);
tsSendResponse(tsChannel, mode, NULL, 0);
printTsStats();
}
int tunerStudioHandleCrcCommand(ts_channel_s *tsChannel, char *data, int incomingPacketSize) {
char command = data[0];
data++;
if (command == TS_HELLO_COMMAND || command == TS_HELLO_COMMAND_DEPRECATED) {
tunerStudioDebug("got Query command");
handleQueryCommand(tsChannel, TS_CRC);
} else if (command == TS_GET_TEXT) {
handleGetText(tsChannel);
} else if (command == TS_EXECUTE) {
handleExecuteCommand(tsChannel, data, incomingPacketSize - 1);
} else if (command == TS_OUTPUT_COMMAND) {
handleOutputChannelsCommand(tsChannel, TS_CRC);
} else if (command == TS_PAGE_COMMAND) {
uint16_t page = *(uint16_t *) data;
handlePageSelectCommand(tsChannel, TS_CRC, page);
} else if (command == TS_CHUNK_WRITE_COMMAND) {
currentPageId = *(uint16_t *) data;
uint16_t offset = *(uint16_t *) (data + 2);
uint16_t count = *(uint16_t *) (data + 4);
handleWriteChunkCommand(tsChannel, TS_CRC, offset, count, data + sizeof(TunerStudioWriteChunkRequest));
} else if (command == TS_SINGLE_WRITE_COMMAND) {
uint16_t page = *(uint16_t *) data;
uint16_t offset = *(uint16_t *) (data + 2);
uint8_t value = data[4];
handleWriteValueCommand(tsChannel, TS_CRC, page, offset, value);
} else if (command == TS_CRC_CHECK_COMMAND) {
uint16_t page = *(uint16_t *) data;
uint16_t offset = *(uint16_t *) (data + 2);
uint16_t count = *(uint16_t *) (data + 4);
handleCrc32Check(tsChannel, TS_CRC, page, offset, count);
} else if (command == TS_BURN_COMMAND) {
uint16_t page = *(uint16_t *) data;
handleBurnCommand(tsChannel, TS_CRC, page);
} else if (command == TS_READ_COMMAND) {
uint16_t page = *(uint16_t *) data;
uint16_t offset = *(uint16_t *) (data + 2);
uint16_t count = *(uint16_t *) (data + 4);
handlePageReadCommand(tsChannel, TS_CRC, page, offset, count);
} else if (command == 't' || command == 'T') {
handleTestCommand(tsChannel);
} else if (command == TS_LEGACY_HELLO_COMMAND) {
/**
* 'Q' is the query command used for compatibility with older ECUs
*/
tunerStudioDebug("ignoring Q");
} else if (command == TS_COMMAND_F) {
tunerStudioDebug("ignoring F");
/**
* http://www.msextra.com/forums/viewtopic.php?f=122&t=48327
* Response from TS support: This is an optional command *
* "The F command is used to find what ini. file needs to be loaded in TunerStudio to match the controller.
* If you are able to just make your firmware ignore the command that would work.
* Currently on some firmware versions the F command is not used and is just ignored by the firmware as a unknown command."
*/
} else {
tunerStudioError("ERROR: ignoring unexpected command");
return false;
}
return true;
}
void handlePageReadCommand(ts_channel_s *tsChannel, ts_response_format_e mode, uint16_t pageId, uint16_t offset,
uint16_t count) {
tsState.readPageCommandsCounter++;
currentPageId = pageId;
#if EFI_TUNER_STUDIO_VERBOSE
scheduleMsg(&tsLogger, "READ m=%d p=%d o=%d c=%d", mode, (int) currentPageId, offset, count);
printTsStats();
#endif
if (currentPageId >= PAGE_COUNT) {
// something is not right here
currentPageId = 0;
tunerStudioError("ERROR: invalid page number");
return;
}
int size = getTunerStudioPageSize(currentPageId);
if (size < offset + count) {
scheduleMsg(&tsLogger, "invalid offset/count %d/%d", offset, count);
sendErrorCode(tsChannel);
return;
}
const uint8_t *addr = (const uint8_t *) (getWorkingPageAddr(currentPageId) + offset);
tsSendResponse(tsChannel, mode, addr, count);
#if EFI_TUNER_STUDIO_VERBOSE
scheduleMsg(&tsLogger, "Sending %d done", count);
#endif
}
/**
* 'Write' command receives a single value at a given offset
* @note Writing values one by one is pretty slow
*/
void handleWriteValueCommand(ts_channel_s *tsChannel, ts_response_format_e mode, uint16_t page, uint16_t offset,
uint8_t value) {
tsState.writeValueCommandCounter++;
currentPageId = page;
tunerStudioDebug("got W (Write)"); // we can get a lot of these
#if EFI_TUNER_STUDIO_VERBOSE
// scheduleMsg(logger, "Page number %d\r\n", pageId); // we can get a lot of these
#endif
// int size = sizeof(TunerStudioWriteValueRequest);
// scheduleMsg(logger, "Reading %d\r\n", size);
if (offset > getTunerStudioPageSize(currentPageId)) {
tunerStudioError("ERROR: out of range2");
scheduleMsg(&tsLogger, "ERROR offset %d", offset);
offset = 0;
return;
}
efitimems_t nowMs = currentTimeMillis();
if (nowMs - previousWriteReportMs > 5) {
previousWriteReportMs = nowMs;
scheduleMsg(&tsLogger, "page %d offset %d: value=%d", currentPageId, offset, value);
}
getWorkingPageAddr(currentPageId)[offset] = value;
yellowMagic(currentPageId, offset, 1);
// scheduleMsg(logger, "va=%d", configWorkingCopy.boardConfiguration.idleValvePin);
}
void handlePageReadCommand(ts_response_format_e mode, uint16_t pageId, uint16_t offset, uint16_t count) {
tsState.readPageCommandsCounter++;
tunerStudioDebug("got R (Read page)");
tsState.currentPageId = pageId;
#if EFI_TUNER_STUDIO_VERBOSE
scheduleMsg(&logger, "Page requested: page %d offset=%d count=%d", (int)tsState.currentPageId, offset, count);
#endif
if (tsState.currentPageId > MAX_PAGE_ID) {
scheduleMsg(&logger, "invalid Page number %x", tsState.currentPageId);
// something is not right here
tsState.currentPageId = 0;
tunerStudioError("ERROR: invalid page");
return;
}
int size = getTunerStudioPageSize(tsState.currentPageId);
if (size < offset + count) {
scheduleMsg(&logger, "invalid offset/count %d/%d", offset, count);
sendErrorCode();
return;
}
const uint8_t *addr = (const uint8_t *) (getWorkingPageAddr(tsState.currentPageId) + offset);
tsSendResponse(mode, addr, count);
#if EFI_TUNER_STUDIO_VERBOSE
scheduleMsg(&logger, "Sending %d done", count);
#endif
}
/**
* This command is needed to make the whole transfer a bit faster
* @note See also handleWriteValueCommand
*/
void handleWriteChunkCommand(ts_response_format_e mode, short offset, short count, void *content) {
tsState.writeChunkCommandCounter++;
scheduleMsg(&logger, "receiving page %d chunk offset %d size %d", tsState.currentPageId, offset, count);
if (offset > getTunerStudioPageSize(tsState.currentPageId)) {
scheduleMsg(&logger, "ERROR offset %d", offset);
tunerStudioError("ERROR: out of range");
offset = 0;
}
if (count > getTunerStudioPageSize(tsState.currentPageId)) {
tunerStudioError("ERROR: unexpected count");
scheduleMsg(&logger, "ERROR count %d", count);
count = 0;
}
uint8_t * addr = (uint8_t *) (getWorkingPageAddr(tsState.currentPageId) + offset);
memcpy(addr, content, count);
tsSendResponse(mode, NULL, 0);
}
/**
* @return true if legacy command was processed, false otherwise
*/
bool handlePlainCommand(ts_channel_s *tsChannel, uint8_t command) {
if (command == TS_HELLO_COMMAND || command == TS_HELLO_COMMAND_DEPRECATED) {
scheduleMsg(&tsLogger, "Got naked Query command");
handleQueryCommand(tsChannel, TS_PLAIN);
return true;
} else if (command == 't' || command == 'T') {
handleTestCommand(tsChannel);
return true;
} else if (command == TS_PAGE_COMMAND) {
int recieved = chnReadTimeout(tsChannel->channel, (uint8_t * )&pageIn, sizeof(pageIn), TS_READ_TIMEOUT);
if (recieved != sizeof(pageIn)) {
tunerStudioError("ERROR: not enough for PAGE");
return true;
}
handlePageSelectCommand(tsChannel, TS_PLAIN, pageIn);
return true;
} else if (command == TS_BURN_COMMAND) {
scheduleMsg(&tsLogger, "Got naked BURN");
uint16_t page;
int recieved = chnReadTimeout(tsChannel->channel, (uint8_t * )&page, sizeof(page), TS_READ_TIMEOUT);
if (recieved != sizeof(page)) {
tunerStudioError("ERROR: Not enough for plain burn");
return true;
}
handleBurnCommand(tsChannel, TS_PLAIN, page);
return true;
} else if (command == TS_CHUNK_WRITE_COMMAND) {
scheduleMsg(&tsLogger, "Got naked CHUNK_WRITE");
int recieved = chnReadTimeout(tsChannel->channel, (uint8_t * )&writeChunkRequest, sizeof(writeChunkRequest),
TS_READ_TIMEOUT);
if (recieved != sizeof(writeChunkRequest)) {
scheduleMsg(&tsLogger, "ERROR: Not enough for plain chunk write header: %d", recieved);
tsState.errorCounter++;
return true;
}
recieved = chnReadTimeout(tsChannel->channel, (uint8_t * )&tsChannel->crcReadBuffer, writeChunkRequest.count,
TS_READ_TIMEOUT);
if (recieved != writeChunkRequest.count) {
scheduleMsg(&tsLogger, "ERROR: Not enough for plain chunk write content: %d while expecting %d", recieved,
writeChunkRequest.count);
tsState.errorCounter++;
return true;
}
currentPageId = writeChunkRequest.page;
handleWriteChunkCommand(tsChannel, TS_PLAIN, writeChunkRequest.offset, writeChunkRequest.count,
(uint8_t *) &tsChannel->crcReadBuffer);
return true;
} else if (command == TS_READ_COMMAND) {
//scheduleMsg(logger, "Got naked READ PAGE???");
int recieved = chnReadTimeout(tsChannel->channel, (uint8_t * )&readRequest, sizeof(readRequest),
TS_READ_TIMEOUT);
if (recieved != sizeof(readRequest)) {
tunerStudioError("Not enough for plain read header");
return true;
}
handlePageReadCommand(tsChannel, TS_PLAIN, readRequest.page, readRequest.offset, readRequest.count);
return true;
} else if (command == TS_OUTPUT_COMMAND) {
//scheduleMsg(logger, "Got naked Channels???");
handleOutputChannelsCommand(tsChannel, TS_PLAIN);
return true;
} else if (command == TS_LEGACY_HELLO_COMMAND) {
tunerStudioDebug("ignoring LEGACY_HELLO_COMMAND");
return true;
} else if (command == TS_COMMAND_F) {
tunerStudioDebug("not ignoring F");
tunerStudioWriteData(tsChannel, (const uint8_t *) PROTOCOL, strlen(PROTOCOL));
return true;
} else {
return false;
}
}
void runBinaryProtocolLoop(ts_channel_s *tsChannel, bool_t isConsoleRedirect) {
int wasReady = false;
while (true) {
int isReady = ts_serial_ready(isConsoleRedirect);
if (!isReady) {
chThdSleepMilliseconds(10);
wasReady = false;
continue;
}
if (!wasReady) {
wasReady = true;
// scheduleSimpleMsg(&logger, "ts channel is now ready ", hTimeNow());
}
tsState.tsCounter++;
int recieved = chnReadTimeout(tsChannel->channel, &firstByte, 1, TS_READ_TIMEOUT);
if (recieved != 1) {
// tunerStudioError("ERROR: no command");
continue;
}
// scheduleMsg(logger, "Got first=%x=[%c]", firstByte, firstByte);
if (handlePlainCommand(tsChannel, firstByte))
continue;
recieved = chnReadTimeout(tsChannel->channel, &secondByte, 1, TS_READ_TIMEOUT);
if (recieved != 1) {
tunerStudioError("ERROR: no second");
continue;
}
// scheduleMsg(logger, "Got secondByte=%x=[%c]", secondByte, secondByte);
uint32_t incomingPacketSize = firstByte * 256 + secondByte;
if (incomingPacketSize == BINARY_SWITCH_TAG) {
// we are here if we get a binary switch request while already in binary mode. We will just ignore it.
tunerStudioWriteData(tsChannel, (const uint8_t *) &BINARY_RESPONSE, 2);
continue;
}
if (incomingPacketSize == 0 || incomingPacketSize > (sizeof(tsChannel->crcReadBuffer) - CRC_WRAPPING_SIZE)) {
scheduleMsg(&tsLogger, "TunerStudio: invalid size: %d", incomingPacketSize);
tunerStudioError("ERROR: CRC header size");
sendErrorCode(tsChannel);
continue;
}
recieved = chnReadTimeout(tsChannel->channel, (uint8_t* )tsChannel->crcReadBuffer, 1, TS_READ_TIMEOUT);
if (recieved != 1) {
tunerStudioError("ERROR: did not receive command");
continue;
}
char command = tsChannel->crcReadBuffer[0];
if (!isKnownCommand(command)) {
scheduleMsg(&tsLogger, "unexpected command %x", command);
sendErrorCode(tsChannel);
continue;
}
// scheduleMsg(logger, "TunerStudio: reading %d+4 bytes(s)", incomingPacketSize);
recieved = chnReadTimeout(tsChannel->channel, (uint8_t * ) (tsChannel->crcReadBuffer + 1),
incomingPacketSize + CRC_VALUE_SIZE - 1, TS_READ_TIMEOUT);
int expectedSize = incomingPacketSize + CRC_VALUE_SIZE - 1;
if (recieved != expectedSize) {
scheduleMsg(&tsLogger, "got ONLY %d for packet size %d/%d for command %c", recieved, incomingPacketSize,
expectedSize, command);
tunerStudioError("ERROR: not enough");
continue;
}
uint32_t expectedCrc = *(uint32_t*) (tsChannel->crcReadBuffer + incomingPacketSize);
expectedCrc = SWAP_UINT32(expectedCrc);
uint32_t actualCrc = crc32(tsChannel->crcReadBuffer, incomingPacketSize);
if (actualCrc != expectedCrc) {
scheduleMsg(&tsLogger, "TunerStudio: CRC %x %x %x %x", tsChannel->crcReadBuffer[incomingPacketSize + 0],
tsChannel->crcReadBuffer[incomingPacketSize + 1], tsChannel->crcReadBuffer[incomingPacketSize + 2],
tsChannel->crcReadBuffer[incomingPacketSize + 3]);
scheduleMsg(&tsLogger, "TunerStudio: command %c actual CRC %x/expected %x", tsChannel->crcReadBuffer[0],
actualCrc, expectedCrc);
tunerStudioError("ERROR: CRC issue");
continue;
}
// scheduleMsg(logger, "TunerStudio: P00-07 %x %x %x %x %x %x %x %x", crcIoBuffer[0], crcIoBuffer[1],
// crcIoBuffer[2], crcIoBuffer[3], crcIoBuffer[4], crcIoBuffer[5], crcIoBuffer[6], crcIoBuffer[7]);
int success = tunerStudioHandleCrcCommand(tsChannel, tsChannel->crcReadBuffer, incomingPacketSize);
if (!success)
print("got unexpected TunerStudio command %x:%c\r\n", command, command);
}
}
static msg_t tsThreadEntryPoint(void *arg) {
(void) arg;
chRegSetThreadName("tunerstudio thread");
int wasReady = false;
while (true) {
int isReady = ts_serail_ready();
if (!isReady) {
chThdSleepMilliseconds(10);
wasReady = false;
continue;
}
if (!wasReady) {
wasReady = TRUE;
// scheduleSimpleMsg(&logger, "ts channel is now ready ", hTimeNow());
}
tsCounter++;
int recieved = chSequentialStreamRead(getTsSerialDevice(), &firstByte, 1);
if (recieved != 1) {
tunerStudioError("ERROR: no command");
continue;
}
// scheduleMsg(&logger, "Got first=%x=[%c]", firstByte, firstByte);
if (handlePlainCommand(firstByte))
continue;
recieved = chSequentialStreamRead(getTsSerialDevice(), &secondByte, 1);
if (recieved != 1) {
tunerStudioError("ERROR: no second");
continue;
}
// scheduleMsg(&logger, "Got secondByte=%x=[%c]", secondByte, secondByte);
uint32_t incomingPacketSize = firstByte * 256 + secondByte;
if (incomingPacketSize == 0 || incomingPacketSize > (sizeof(crcIoBuffer) - CRC_WRAPPING_SIZE)) {
scheduleMsg(&logger, "TunerStudio: invalid size: %d", incomingPacketSize);
tunerStudioError("ERROR: size");
sendErrorCode();
continue;
}
recieved = chnReadTimeout(getTsSerialDevice(), crcIoBuffer, 1, MS2ST(TS_READ_TIMEOUT));
if (recieved != 1) {
tunerStudioError("ERROR: did not receive command");
continue;
}
char command = crcIoBuffer[0];
if (!isKnownCommand(command)) {
scheduleMsg(&logger, "unexpected command %x", command);
sendErrorCode();
continue;
}
// scheduleMsg(&logger, "TunerStudio: reading %d+4 bytes(s)", incomingPacketSize);
recieved = chnReadTimeout(getTsSerialDevice(), (uint8_t * ) (crcIoBuffer + 1), incomingPacketSize + CRC_VALUE_SIZE - 1,
MS2ST(TS_READ_TIMEOUT));
int expectedSize = incomingPacketSize + CRC_VALUE_SIZE - 1;
if (recieved != expectedSize) {
scheduleMsg(&logger, "got ONLY %d for packet size %d/%d for command %c", recieved, incomingPacketSize,
expectedSize, command);
tunerStudioError("ERROR: not enough");
continue;
}
uint32_t expectedCrc = *(uint32_t*) (crcIoBuffer + incomingPacketSize);
expectedCrc = SWAP_UINT32(expectedCrc);
uint32_t actualCrc = crc32(crcIoBuffer, incomingPacketSize);
if (actualCrc != expectedCrc) {
scheduleMsg(&logger, "TunerStudio: CRC %x %x %x %x", crcIoBuffer[incomingPacketSize + 0],
crcIoBuffer[incomingPacketSize + 1], crcIoBuffer[incomingPacketSize + 2],
crcIoBuffer[incomingPacketSize + 3]);
scheduleMsg(&logger, "TunerStudio: command %c actual CRC %x/expected %x", crcIoBuffer[0], actualCrc,
expectedCrc);
tunerStudioError("ERROR: CRC issue");
continue;
}
// scheduleMsg(&logger, "TunerStudio: P00-07 %x %x %x %x %x %x %x %x", crcIoBuffer[0], crcIoBuffer[1],
// crcIoBuffer[2], crcIoBuffer[3], crcIoBuffer[4], crcIoBuffer[5], crcIoBuffer[6], crcIoBuffer[7]);
int success = tunerStudioHandleCrcCommand(crcIoBuffer, incomingPacketSize);
if (!success)
print("got unexpected TunerStudio command %x:%c\r\n", command, command);
}
#if defined __GNUC__
return 0;
#endif
}
