Biderectional l2test

@@ -54,7 +54,9 @@
 	DUMP,
 	CONNECT,
 	CRECV,
-	LSEND
+	LSEND,
+	BILISTEN,
+	BICONNECT
 };
 
 unsigned char *buf;

@@ -74,6 +76,13 @@
 int auth = 0;
 int encrypt = 0;
 
+
+uint32_t verify_data(unsigned char *buf, int r, uint32_t seq);
+void bi_send_recv(int s);
+
+unsigned char *bi_recv_buf;
+int bi_recv_count=10000;
+
 float tv2fl(struct timeval tv)
 {
 	return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);

@@ -246,9 +255,7 @@
 		gettimeofday(&tv_beg,NULL);
 		total = 0;
 		while (total < data_size) {
-			uint32_t sq;
-			uint16_t l;
-			int i,r;
+			int r;
 
 			if ((r = recv(s, buf, data_size, 0)) <= 0) {
 				if (r < 0)

@@ -257,26 +264,7 @@
 				return;	
 			}
 
-			/* Check sequence */
-			sq = btohl(*(uint32_t *)buf);
-			if (seq != sq) {
-				syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
-				seq = sq;
-			}
-			seq++;
-			
-			/* Check length */
-			l = btohs(*(uint16_t *)(buf+4));
-			if (r != l) {
-				syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
-				continue;
-			}
-			
-			/* Verify data */	
-			for (i=6; i < r; i++) {
-				if (buf[i] != 0x7f)
-					syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
-			}
+			seq = verify_data(buf, r, seq);
 
 			total += r;
 		}

@@ -311,6 +299,101 @@
 	}
 }
 
+void bi_connect_mode(char* svr) {
+  int s;
+  if( (s = do_connect(svr)) < 0 )
+    exit(1);
+  bi_send_recv(s);
+}
+
+void bi_send_recv(int s) {
+  int i, r;
+  long total;
+  uint32_t recv_seq, send_seq;
+  struct timeval tv_beg, tv_end, tv_diff;
+  fd_set rset, wset;
+
+  /* Incoming packets are limited by the -I option */
+  if (!(bi_recv_buf = malloc(imtu))) {
+    perror("Can't allocate data buffer");
+    exit(1);
+  }
+  for(i=6; i < imtu; i++) buf[i]=0x7f;
+
+  send_seq = 0;
+  recv_seq = 0;
+  total = 0;
+  FD_ZERO(&rset);
+  FD_ZERO(&wset); 
+  gettimeofday(&tv_beg,NULL);
+  while(1) {
+    FD_SET(s, &rset);
+    FD_SET(s, &wset);
+    if (select(s+1, &rset, &wset, NULL, NULL)<0){
+      syslog(LOG_ERR, "Select failed. %s(%d)", strerror(errno), errno);
+      exit(-1);
+    }
+
+    if(FD_ISSET(s, &wset)) {
+      *(uint32_t *)buf = htobl(send_seq++);
+      *(uint16_t *)(buf+4) = htobs(data_size);
+	
+      if( send(s, buf, data_size, 0) <= 0 ) {
+	syslog(LOG_ERR, "Send failed. %s(%d)", strerror(errno), errno);
+	exit(1);
+      }
+    }
+    if(FD_ISSET(s, &rset)) {
+      if ((r = recv(s, bi_recv_buf, imtu, 0)) < 0) {
+	if (r < 0)
+	  syslog(LOG_ERR, "Read failed. %s(%d)", strerror(errno), errno);
+	return;
+      }
+      recv_seq = verify_data(bi_recv_buf, r, recv_seq);
+      total += r;
+
+      if(total >= bi_recv_count) {
+	gettimeofday(&tv_end,NULL);
+	timersub(&tv_end,&tv_beg,&tv_diff);
+	syslog(LOG_INFO,"Received %ld bytes in %.2f sec, %.2f kB/s", total,
+	       tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
+	tv_beg=tv_end;
+	total=0;
+      }
+    }
+  }
+}
+
+uint32_t verify_data(unsigned char* buf, int r, uint32_t seq){
+  uint32_t sq;
+  uint16_t l;
+  int i;
+
+  /* Check sequence */
+  sq = btohl(*(uint32_t *)buf);
+  if (seq != sq) {
+    printf("seq missmatch: %d -> %d", seq, sq);
+    syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
+    seq = sq;
+  }
+  seq++;
+			
+  /* Check length */
+  l = btohs(*(uint16_t *)(buf+4));
+  if (r != l) {
+    syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
+    goto done;
+  }
+		
+  /* Verify data */	
+  for (i=6; i < r; i++) {
+    if (buf[i] != 0x7f)
+      syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
+  }
+ done:
+  return seq;
+}
+
 void reconnect_mode(char *svr)
 {
 	while(1){

@@ -361,7 +444,9 @@
 		"\t-u connect and receive\n"
 		"\t-n connect and be silent\n"
 		"\t-c connect, disconnect, connect, ...\n"
-		"\t-m multiple connects\n");
+		"\t-m multiple connects\n"
+                "\t-x biderectional send/recv - listen\n"
+                "\t-y biderectional send/recv - connect\n");
 
 	printf("Options:\n"
 		"\t[-b bytes] [-S bdaddr] [-P psm]\n"

@@ -381,7 +466,7 @@
 
 	mode = RECV; need_addr = 0;
 	
-	while ((opt=getopt(argc,argv,"rdscuwmnb:P:I:O:S:MAE")) != EOF) {
+	while ((opt=getopt(argc,argv,"rdscuwmnxyxb:P:I:O:S:MAE")) != EOF) {
 		switch(opt) {
 		case 'r':
 			mode = RECV;

@@ -424,6 +509,15 @@
 			data_size = atoi(optarg);
 			break;
 
+		case 'x':
+		        mode=BILISTEN;
+			break;
+
+		case 'y':
+		        mode=BICONNECT;
+			need_addr = 1;
+			break;
+
 		case 'S':
 			baswap(&bdaddr, strtoba(optarg));
 			break;

@@ -476,6 +570,14 @@
 	openlog("l2test", LOG_PERROR | LOG_PID, LOG_LOCAL0);
 
 	switch( mode ){
+	        case BILISTEN:
+		        do_listen(bi_send_recv);
+ 		        break;
+
+	        case BICONNECT:
+	                bi_connect_mode(argv[optind]);
+ 		        break;
+
 		case RECV:
 			do_listen(recv_mode);
 			break;