From: Michal Sojka <sojka@os.inf.tu-dresden.de>
Date: Tue, 20 Sep 2011 00:19:45 +0000 (+0200)
Subject: latester: Add calculation of stuff bits
X-Git-Tag: fix-allnoconfig~191
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/can-benchmark.git/commitdiff_plain/784577fb17d4acb59be8c9810312ec0b53136ff8

latester: Add calculation of stuff bits
---

diff --git a/latester/latester.c b/latester/latester.c
index 61b2b43..bf0eff0 100644
--- a/latester/latester.c
+++ b/latester/latester.c
@@ -1,6 +1,6 @@
 /**************************************************************************/
 /* CAN latency tester                                                     */
-/* Copyright (C) 2010 Michal Sojka, DCE FEE CTU Prague                    */
+/* Copyright (C) 2010, 2011 Michal Sojka, DCE FEE CTU Prague                    */
 /* License: GPLv2     	     	    	    				  */
 /**************************************************************************/
 
@@ -158,6 +158,89 @@ static inline char *tstamp_str(const void *ctx, struct timespec *tstamp)
 			       tstamp->tv_sec, tstamp->tv_nsec/1000);
 }
 
+unsigned calc_stuff_bits(struct can_frame *frame) {
+	uint32_t bitmap[4];
+	unsigned start = 0, end;
+	uint32_t mask, ones = 0, basemask = 0xf0000000;
+	unsigned stuffed = 0;
+	memset(bitmap, 0, sizeof(bitmap));
+
+	if (frame->can_id & CAN_EFF_FLAG) {
+		bitmap[0] =
+			(frame->can_id >> 25);
+		bitmap[1] =
+			(frame->can_id >> 18) << 25             |
+			3 << 23					|
+			(frame->can_id & 0x3ffff) << 7		|
+			(!!(frame->can_id & CAN_RTR_FLAG)) << 6 |
+			0 << 4 					|
+			frame->can_dlc & 0xf;
+		bitmap[2] = htonl(((uint32_t*)frame->data)[0]);
+		bitmap[3] = htonl(((uint32_t*)frame->data)[1]);
+		start = 28;
+		end = 64 + 8*frame->can_dlc;
+	} else {
+		bitmap[0] =
+			(frame->can_id << 7) |
+			(!!(frame->can_id & CAN_RTR_FLAG)) << 6 |
+			0 << 4 					|
+			frame->can_dlc & 0xf;
+		bitmap[1] = htonl(((uint32_t*)frame->data)[0]);
+		bitmap[2] = htonl(((uint32_t*)frame->data)[1]);
+		start = 14;
+		end = 32 + 8*frame->can_dlc;
+	}
+	/* TODO: Calc stuff bits in CRC */
+	while (start < end) {
+		mask = basemask >> (start & 0x1f);
+		while (1) {
+			ones = ones ? mask : 0;
+			uint32_t chunk = (bitmap[start >> 5] & mask) ^ ones;
+			//printf("start=%d  bitmap=0x%08x  mask=0x%08x  ones=0x%08x  chunk=0x%08x\n", start, bitmap[start >> 5], mask, ones, chunk);
+			if (chunk) {
+				unsigned change = __builtin_clz(chunk);
+				start = start & ~0x1f | change;
+				basemask = 0xf8000000;
+			} else {
+				unsigned oldstart = start;
+				start += __builtin_popcount(mask);
+				mask = (oldstart & 0x1f) ? basemask << (-oldstart & 0x1f) : 0;
+				//printf("oldstart=%d  shl=%d  mask=0x%08x\n", oldstart, -oldstart & 0x1f, mask);
+				if (mask && start < end)
+					continue;
+				if (start <= end && !mask) {
+					stuffed++;
+					basemask = 0xf0000000;
+					printf("stuffed %d\n", !ones);
+				}
+			}
+			break;
+		}
+		ones = !ones;
+	}
+	printf ("STUFFED %d BITS\n", stuffed);
+}
+
+unsigned calc_frame_txtime(struct can_frame *frame) {
+	return calc_stuff_bits(frame) +
+		1 +		/* SOF */
+		11 +		/* ID A */
+		((frame->can_id & CAN_EFF_FLAG) ?
+		 1 +		/* SRR */
+		 1 +		/* IDE */
+		 18 +		/* ID B */
+		 1 +		/* RTR */
+		 2		/* r1, r0 */
+		 :
+		 1 +		/* rtr */
+		 2) +		/* ide, r0 */
+		4 + 		/* dlc */
+		8*frame->can_dlc +
+		15 + 		/* CRC */
+		3 + 		/* CRC del, ACK, ACK del */
+		7;		/* EOF */
+}
+
 void msg_info_print(FILE *f, struct msg_info *mi)
 {
 	struct timespec diff;
@@ -173,20 +256,22 @@ void msg_info_print(FILE *f, struct msg_info *mi)
 
 	switch (num_interfaces) {
 	case 2:
-		fprintf(f, "%ld: %s %s -> %s (%s) %s = %s (%s)\n",
+		fprintf(f, "%ld: %s %s -> %s (%s) %s = %s (%s) %d\n",
 			num, S(mi->ts_sent), sent, S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
-		       DIFF(mi->ts_sent, mi->ts_rx_final_kern),
-		       DIFF(mi->ts_sent, mi->ts_rx_final));
+			DIFF(mi->ts_sent, mi->ts_rx_final_kern),
+			DIFF(mi->ts_sent, mi->ts_rx_final),
+			calc_frame_txtime(mi->received));
 		break;
 	case 3:
-		fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s)\n",
+		fprintf(f, "%ld: %s %s -> %s (%s) -> %s (%s) %s = %s (%s), %s (%s) %d\n",
 			num, S(mi->ts_sent), sent,
 			S(mi->ts_rx_onwire_kern), S(mi->ts_rx_onwire),
 			S(mi->ts_rx_final_kern), S(mi->ts_rx_final), received,
 			DIFF(mi->ts_sent, mi->ts_rx_onwire_kern),
 			DIFF(mi->ts_sent, mi->ts_rx_onwire),
 			DIFF(mi->ts_rx_onwire_kern, mi->ts_rx_final_kern),
-			DIFF(mi->ts_rx_onwire, mi->ts_rx_final));
+			DIFF(mi->ts_rx_onwire, mi->ts_rx_final),
+			calc_frame_txtime(mi->received));
 		break;
 	}
 #undef S