ngscopeclient · Nov 23, 2020 · Nov 23, 2020
Showing with 125 additions and 43 deletions.

+1 −1 scopeprotocols/USB2PCSDecoder.cpp

+8 −7 scopeprotocols/USB2PMADecoder.cpp

+109 −33 scopeprotocols/USB2PacketDecoder.cpp

+7 −2 scopeprotocols/USB2PacketDecoder.h
diff --git a/scopeprotocols/USB2PCSDecoder.cpp b/scopeprotocols/USB2PCSDecoder.cpp
@@ -374,7 +374,7 @@ void USB2PCSDecoder::RefreshIterationData(
 	{
 		//If we're not two UIs long, we have a problem
 		//TODO: handle reset
-		if(round(sample_width_ui) != 2)
+		if(sample_width_ui < 1.2)
 		{
 			cap->m_offsets.push_back(din->m_offsets[nin]);
 			cap->m_durations.push_back(din->m_durations[nin]);

diff --git a/scopeprotocols/USB2PMADecoder.cpp b/scopeprotocols/USB2PMADecoder.cpp
@@ -123,29 +123,30 @@ void USB2PMADecoder::Refresh()
 	{
 		bool bp = (din_p->m_samples[i] > 0.4);
 		bool bn = (din_n->m_samples[i] > 0.4);
+		float vdiff = din_p->m_samples[i] - din_n->m_samples[i];
 
 		USB2PMASymbol::SegmentType type = USB2PMASymbol::TYPE_SE1;
-		if(bp && bn)
-			type = USB2PMASymbol::TYPE_SE1;
-		else if(!bp && !bn)
-			type = USB2PMASymbol::TYPE_SE0;
-		else
+		if(fabs(vdiff) > 0.4)
 		{
 			if(speed == 1)
 			{
-				if(bp && !bn)
+				if(vdiff > 0)
 					type = USB2PMASymbol::TYPE_J;
 				else
 					type = USB2PMASymbol::TYPE_K;
 			}
 			else
 			{
-				if(bp && !bn)
+				if(vdiff > 0)
 					type = USB2PMASymbol::TYPE_K;
 				else
 					type = USB2PMASymbol::TYPE_J;
 			}
 		}
+		else if(bp && bn)
+			type = USB2PMASymbol::TYPE_SE1;
+		else
+			type = USB2PMASymbol::TYPE_SE0;
 
 		//First sample goes as-is
 		if(cap->m_samples.empty())

diff --git a/scopeprotocols/USB2PacketDecoder.cpp b/scopeprotocols/USB2PacketDecoder.cpp
@@ -141,6 +141,9 @@ void USB2PacketDecoder::Refresh()
 	//Decode stuff
 	uint8_t last = 0;
 	uint64_t last_offset;
+	uint8_t crc5_in[2] = {0};
+	uint8_t packet_crc5;
+	vector<uint8_t> packet_data;
 	for(size_t i=0; i<len; i++)
 	{
 		auto& sin = din->m_samples[i];
@@ -231,6 +234,7 @@ void USB2PacketDecoder::Refresh()
 					case USB2PacketSymbol::PID_DATA2:
 					case USB2PacketSymbol::PID_MDATA:
 						state = STATE_DATA;
+						packet_data.clear();
 						break;
 				}
 
@@ -254,6 +258,7 @@ void USB2PacketDecoder::Refresh()
 				cap->m_durations.push_back(din->m_durations[i]);
 				cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_ADDR, sin.m_data & 0x7f));
 
+				crc5_in[0] = sin.m_data;
 				last = sin.m_data;
 
 				state = STATE_TOKEN_1;
@@ -266,9 +271,15 @@ void USB2PacketDecoder::Refresh()
 				cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_ENDP,
 					( last >> 7) | ( (sin.m_data & 0x7) << 1 )));
 
+				//Verify the CRC
+				crc5_in[1] = sin.m_data;
+				packet_crc5 = (sin.m_data >> 3);
 				cap->m_offsets.push_back(din->m_offsets[i] + halfdur);
 				cap->m_durations.push_back(halfdur);
-				cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5, sin.m_data >> 3));
+				if(VerifyCRC5(crc5_in))
+					cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5_GOOD, packet_crc5));
+				else
+					cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5_BAD, packet_crc5));
 
 				state = STATE_END;
 				break;
@@ -277,6 +288,7 @@ void USB2PacketDecoder::Refresh()
 
 				last = sin.m_data;
 				last_offset = din->m_offsets[i];
+				crc5_in[0] = sin.m_data;
 
 				state = STATE_SOF_1;
 				break;
@@ -290,9 +302,14 @@ void USB2PacketDecoder::Refresh()
 						(sin.m_data & 0x7 ) << 8 | last));
 
 				//CRC
+				crc5_in[1] = sin.m_data;
+				packet_crc5 = (sin.m_data >> 3);
 				cap->m_offsets.push_back(din->m_offsets[i] + halfdur);
 				cap->m_durations.push_back(halfdur);
-				cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5, sin.m_data >> 3));
+				if(VerifyCRC5(crc5_in))
+					cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5_GOOD, packet_crc5));
+				else
+					cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC5_BAD, packet_crc5));
 
 				state = STATE_END;
 				break;
@@ -305,6 +322,8 @@ void USB2PacketDecoder::Refresh()
 					cap->m_offsets.push_back(din->m_offsets[i]);
 					cap->m_durations.push_back(din->m_durations[i]);
 					cap->m_samples.push_back(USB2PacketSymbol(USB2PacketSymbol::TYPE_DATA, sin.m_data));
+
+					packet_data.push_back(sin.m_data);
 				}
 
 				//Last two bytes were actually the CRC!
@@ -314,10 +333,20 @@ void USB2PacketDecoder::Refresh()
 					size_t firstoff = cap->m_samples.size() - 2;
 					size_t secondoff = cap->m_samples.size() - 1;
 
+					//Extract the CRC value and remove it from the packet body
+					uint16_t crc16 = (cap->m_samples[firstoff].m_data << 8) | cap->m_samples[secondoff].m_data;
+					packet_data.pop_back();
+					packet_data.pop_back();
+
+					//Calculate the CRC
+					uint16_t crc16_calculated = CalculateCRC16(packet_data);
+					LogDebug("Calculated CRC16: %04x, got: %04x\n", crc16_calculated, crc16);
+
 					cap->m_durations[firstoff] += cap->m_durations[secondoff];
-					cap->m_samples[firstoff] = USB2PacketSymbol(
-						USB2PacketSymbol::TYPE_CRC16,
-						(cap->m_samples[firstoff].m_data << 8) | cap->m_samples[secondoff].m_data);
+					if(crc16 == crc16_calculated)
+						cap->m_samples[firstoff] = USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC16_GOOD, crc16);
+					else
+						cap->m_samples[firstoff] = USB2PacketSymbol(USB2PacketSymbol::TYPE_CRC16_BAD, crc16);
 
 					cap->m_offsets.resize(secondoff);
 					cap->m_durations.resize(secondoff);
@@ -339,6 +368,59 @@ void USB2PacketDecoder::Refresh()
 	FindPackets(cap);
 }
 
+/**
+	@brief Calculates the USB CRC16
+ */
+uint16_t USB2PacketDecoder::CalculateCRC16(const std::vector<uint8_t>& data)
+{
+	uint16_t poly = 0xa001;
+	uint16_t crc = 0xffff;
+
+	for(size_t n=0; n<data.size(); n++)
+	{
+		uint8_t d = data[n];
+
+		for(int i=0; i<8; i++)
+		{
+			bool b = ( crc ^ (d >> i) ) & 1;
+			crc >>= 1;
+			if(b)
+				crc ^= poly;
+		}
+	}
+
+	return ~( (crc << 8) | ( (crc >> 8) & 0xff) );
+}
+
+/**
+	@brief Table based CRC5 implementation
+
+	Ref: "A Fast Compact CRC5 Checker For Microcontrollers", Michael Joost
+ */
+bool USB2PacketDecoder::VerifyCRC5(uint8_t* data)
+{
+	const uint8_t table4[16] =
+	{
+		0x00, 0x0e, 0x1c, 0x12, 0x11, 0x1f, 0x0d, 0x03,
+		0x0b, 0x05, 0x17, 0x19, 0x1a, 0x14, 0x06, 0x08
+	};
+
+	const uint8_t table0[16] =
+	{
+		0x00, 0x16, 0x05, 0x13, 0x0a, 0x1c, 0x0f, 0x19,
+		0x14, 0x02, 0x11, 0x07, 0x1e, 0x08, 0x1b, 0x0d
+	};
+
+	uint8_t crc = 0x1f;
+	for(int i=0; i<2; i++)
+	{
+		uint8_t b = data[i] ^ crc;
+		crc = table4[b & 0xf] ^ table0[(b >> 4) & 0xf];
+	}
+
+	return (crc == 6);
+}
+
 void USB2PacketDecoder::FindPackets(USB2PacketWaveform* cap)
 {
 	ClearPackets();
@@ -395,7 +477,7 @@ void USB2PacketDecoder::DecodeSof(USB2PacketWaveform* cap, size_t istart, size_t
 	auto scrc = cap->m_samples[icrc];
 	if(snframe.m_type != USB2PacketSymbol::TYPE_NFRAME)
 		return;
-	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5)
+	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5_GOOD)
 		return;
 
 	//Make the packet
@@ -437,11 +519,8 @@ void USB2PacketDecoder::DecodeSetup(USB2PacketWaveform* cap, size_t istart, size
 		LogError("not TYPE_ENDP\n");
 		return;
 	}
-	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5)
-	{
-		LogError("not TYPE_CRC5\n");
+	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5_GOOD)
 		return;
-	}
 
 	//Expect a DATA0 packet next
 	//Should be PID, 8 bytes, CRC16.
@@ -475,11 +554,8 @@ void USB2PacketDecoder::DecodeSetup(USB2PacketWaveform* cap, size_t istart, size
 	}
 	size_t idcrc = i++;
 	auto sdcrc = cap->m_samples[idcrc];
-	if(sdcrc.m_type != USB2PacketSymbol::TYPE_CRC16)
-	{
-		LogError("not CRC16\n");
+	if(sdcrc.m_type != USB2PacketSymbol::TYPE_CRC16_GOOD)
 		return;
-	}
 
 	//Expect ACK/NAK
 	string ack = "";
@@ -596,11 +672,8 @@ void USB2PacketDecoder::DecodeData(USB2PacketWaveform* cap, size_t istart, size_
 		LogError("not TYPE_ENDP\n");
 		return;
 	}
-	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5)
-	{
-		LogError("not TYPE_CRC5\n");
+	if(scrc.m_type != USB2PacketSymbol::TYPE_CRC5_GOOD)
 		return;
-	}
 
 	//Expect minimum DATA, 0 or more data bytes, ACK
 	if(i >= cap->m_samples.size())
@@ -684,11 +757,8 @@ void USB2PacketDecoder::DecodeData(USB2PacketWaveform* cap, size_t istart, size_
 		}
 
 		//Next should be a CRC16
-		else if(s.m_type == USB2PacketSymbol::TYPE_CRC16)
-		{
-			//TODO: verify the CRC
+		else if(s.m_type == USB2PacketSymbol::TYPE_CRC16_GOOD)
 			break;
-		}
 
 		i++;
 	}
@@ -761,9 +831,13 @@ Gdk::Color USB2PacketDecoder::GetColor(int i)
 		case USB2PacketSymbol::TYPE_NFRAME:
 			return m_standardColors[COLOR_DATA];
 
-		case USB2PacketSymbol::TYPE_CRC5:
-		case USB2PacketSymbol::TYPE_CRC16:
-			return m_standardColors[COLOR_CHECKSUM_OK];	//TODO: verify checksum
+		case USB2PacketSymbol::TYPE_CRC5_GOOD:
+		case USB2PacketSymbol::TYPE_CRC16_GOOD:
+			return m_standardColors[COLOR_CHECKSUM_OK];
+
+		case USB2PacketSymbol::TYPE_CRC5_BAD:
+		case USB2PacketSymbol::TYPE_CRC16_BAD:
+			return m_standardColors[COLOR_CHECKSUM_BAD];
 
 		case USB2PacketSymbol::TYPE_DATA:
 			return m_standardColors[COLOR_DATA];
@@ -831,20 +905,22 @@ string USB2PacketDecoder::GetText(int i)
 			break;
 		}
 		case USB2PacketSymbol::TYPE_ADDR:
-			snprintf(tmp, sizeof(tmp), "Dev %d", sample.m_data);
-			return string(tmp);
+			return string("Dev ") + to_string(sample.m_data);
 		case USB2PacketSymbol::TYPE_NFRAME:
-			snprintf(tmp, sizeof(tmp), "Frame %d", sample.m_data);
-			return string(tmp);
+			return string("Frame ") + to_string(sample.m_data);
 		case USB2PacketSymbol::TYPE_ENDP:
-			snprintf(tmp, sizeof(tmp), "EP %d", sample.m_data);
-			return string(tmp);
-		case USB2PacketSymbol::TYPE_CRC5:
+			return string("EP ") + to_string(sample.m_data);
+
+		case USB2PacketSymbol::TYPE_CRC5_GOOD:
+		case USB2PacketSymbol::TYPE_CRC5_BAD:
 			snprintf(tmp, sizeof(tmp), "CRC %02x", sample.m_data);
 			return string(tmp);
-		case USB2PacketSymbol::TYPE_CRC16:
+
+		case USB2PacketSymbol::TYPE_CRC16_GOOD:
+		case USB2PacketSymbol::TYPE_CRC16_BAD:
 			snprintf(tmp, sizeof(tmp), "CRC %04x", sample.m_data);
 			return string(tmp);
+
 		case USB2PacketSymbol::TYPE_DATA:
 			snprintf(tmp, sizeof(tmp), "%02x", sample.m_data);
 			return string(tmp);

diff --git a/scopeprotocols/USB2PacketDecoder.h b/scopeprotocols/USB2PacketDecoder.h
@@ -50,8 +50,10 @@ class USB2PacketSymbol
 		TYPE_PID,
 		TYPE_ADDR,
 		TYPE_ENDP,
-		TYPE_CRC5,
-		TYPE_CRC16,
+		TYPE_CRC5_GOOD,
+		TYPE_CRC5_BAD,
+		TYPE_CRC16_GOOD,
+		TYPE_CRC16_BAD,
 		TYPE_NFRAME,
 		TYPE_DATA,
 		TYPE_ERROR
@@ -125,6 +127,9 @@ class USB2PacketDecoder : public PacketDecoder
 	void DecodeSof(USB2PacketWaveform* cap, size_t istart, size_t& i);
 	void DecodeSetup(USB2PacketWaveform* cap, size_t istart, size_t& i);
 	void DecodeData(USB2PacketWaveform* cap, size_t istart, size_t& i);
+
+	bool VerifyCRC5(uint8_t* data);
+	uint16_t CalculateCRC16(const std::vector<uint8_t>& data);
 };
 
 #endif