Re-applying part of "fix CRC errors #1944"

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<uint32_t*>(&scratchBuffer[size + 3]) = SWAP_UINT32(crc);
+
+	// Write to the underlying stream
+	sr5WriteData(tsChannel, reinterpret_cast<uint8_t*>(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);
 }
 

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
 	 */