From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 7 Nov 2013 23:33:24 +0000 (+0200)
Subject: Insert dummy zero at end of packet if last eight bits are ones
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=01cf3eeba36d410c8a9f21f23f9652e9580e1929;p=model-railway-devices.git

Insert dummy zero at end of packet if last eight bits are ones
---

diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
index a39ce1f..95de445 100644
--- a/arducontrol/mfx.c
+++ b/arducontrol/mfx.c
@@ -114,21 +114,16 @@ static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *
 {
 	uint8_t out_bit = packet->length;
 	uint8_t out_level = mfx_get_output_level(packet);
+	uint8_t ones_count = state->ones_count;
 
 	for(uint8_t i=length; i--; )
 	{
 		uint8_t bit = (bits>>i)&1;
 
-		if(state->ones_count>=8)
-		{
-			++i;
-			bit = 0;
-			state->ones_count = 0;
-		}
-		else if(bit)
-			++state->ones_count;
+		if(bit)
+			++ones_count;
 		else
-			state->ones_count = 0;
+			ones_count = 0;
 
 		if(out_level==bit)
 			bit ^= 3;
@@ -140,8 +135,15 @@ static inline void mfx_encode_bits8_raw(OutputPacket *packet, MfxEncodingState *
 			packet->data[out_bit>>3] |= bit<<(out_bit&7);
 		out_bit += 2;
 		out_level = bit>>1;
+
+		if(ones_count>=8)
+		{
+			bits &= (1<<i)-1;
+			++i;
+		}
 	}
 
+	state->ones_count = ones_count;
 	packet->length = out_bit;
 }