diff --git a/firmware/console/binary/tunerstudio_io.cpp b/firmware/console/binary/tunerstudio_io.cpp index 7d9f0d05aa..74961a790e 100644 --- a/firmware/console/binary/tunerstudio_io.cpp +++ b/firmware/console/binary/tunerstudio_io.cpp @@ -224,13 +224,68 @@ int sr5ReadData(ts_channel_s *tsChannel, uint8_t * buffer, int size) { return sr5ReadDataTimeout(tsChannel, buffer, size, SR5_READ_TIMEOUT); } +static void sr5WriteCrcPacketSmall(ts_channel_s* tsChannel, uint8_t responseCode, const uint8_t* buf, size_t size) { + auto scratchBuffer = tsChannel->scratchBuffer; + + // don't transmit too large a buffer + efiAssertVoid(OBD_PCM_Processor_Fault, size <= BLOCKING_FACTOR + 7, "sr5WriteCrcPacket tried to transmit too large a packet") + + // If transmitting data, copy it in to place in the scratch buffer + // We want to prevent the data changing itself (higher priority threads could write + // tsOutputChannels) during the CRC computation. Instead compute the CRC on our + // local buffer that nobody else will write. + if (size) { + memcpy(scratchBuffer + 3, buf, size); + } + + // Index 0/1 = packet size (big endian) + *(uint16_t*)scratchBuffer = SWAP_UINT16(size + 1); + // Index 2 = response code + scratchBuffer[2] = responseCode; + + // CRC is computed on the responseCode and payload but not length + uint32_t crc = crc32(&scratchBuffer[2], size + 1); // command part of CRC + + // Place the CRC at the end + *reinterpret_cast(&scratchBuffer[size + 3]) = SWAP_UINT32(crc); + + // Write to the underlying stream + sr5WriteData(tsChannel, reinterpret_cast(scratchBuffer), size + 7); +} + +static void sr5WriteCrcPacketLarge(ts_channel_s* tsChannel, uint8_t responseCode, const uint8_t* buf, size_t size) { + uint8_t headerBuffer[3]; + uint8_t crcBuffer[4]; + + *(uint16_t*)headerBuffer = SWAP_UINT16(size + 1); + *(uint8_t*)(headerBuffer + 2) = responseCode; + + // Command part of CRC + uint32_t crc = crc32((void*)(headerBuffer + 2), 1); + // Data part of CRC + crc = crc32inc((void*)buf, crc, size); + *(uint32_t*)crcBuffer = SWAP_UINT32(crc); + + // Write header + sr5WriteData(tsChannel, headerBuffer, sizeof(headerBuffer)); + + // If data, write that + if (size) { + sr5WriteData(tsChannel, buf, size); + } + + // Lastly the CRC footer + sr5WriteData(tsChannel, crcBuffer, sizeof(crcBuffer)); +} /** * Adds size to the beginning of a packet and a crc32 at the end. Then send the packet. */ void sr5WriteCrcPacket(ts_channel_s *tsChannel, uint8_t responseCode, const uint8_t* buf, size_t size) { - uint8_t *writeBuffer = tsChannel->writeBuffer; - uint8_t *crcBuffer = &tsChannel->writeBuffer[3]; + // don't transmit a null buffer... + if (!buf) { + size = 0; + } #if defined(TS_CAN_DEVICE) && defined(TS_CAN_DEVICE_SHORT_PACKETS_IN_ONE_FRAME) // a special case for short packets: we can sent them in 1 frame, without CRC & size, @@ -245,20 +300,16 @@ void sr5WriteCrcPacket(ts_channel_s *tsChannel, uint8_t responseCode, const uint } #endif /* TS_CAN_DEVICE */ - *(uint16_t *) writeBuffer = SWAP_UINT16(size + 1); // packet size including command - *(uint8_t *) (writeBuffer + 2) = responseCode; - - // CRC on whole packet - uint32_t crc = crc32((void *) (writeBuffer + 2), 1); // command part of CRC - crc = crc32inc((void *) buf, crc, (uint32_t) (size)); // combined with packet CRC - - *(uint32_t *) (crcBuffer) = SWAP_UINT32(crc); - - sr5WriteData(tsChannel, writeBuffer, 3); // header - if (size > 0) { - sr5WriteData(tsChannel, (const uint8_t*)buf, size); // body +/* + if (size <= BLOCKING_FACTOR + 7) { + // small packets use small packet optimization + sr5WriteCrcPacketSmall(tsChannel, responseCode, buf, size); + } else { +*/ + sr5WriteCrcPacketLarge(tsChannel, responseCode, buf, size); +/* } - sr5WriteData(tsChannel, crcBuffer, 4); // CRC footer +*/ sr5FlushData(tsChannel); } diff --git a/firmware/console/binary/tunerstudio_io.h b/firmware/console/binary/tunerstudio_io.h index 3d0a4c5a0f..662cd9e566 100644 --- a/firmware/console/binary/tunerstudio_io.h +++ b/firmware/console/binary/tunerstudio_io.h @@ -26,7 +26,6 @@ struct ts_channel_s { #if ! EFI_UNIT_TEST BaseChannel * channel = nullptr; #endif - uint8_t writeBuffer[7]; // size(2 bytes) + response(1 byte) + crc32 (4 bytes) /** * See 'blockingFactor' in rusefi.ini */