Index: source/ariba/utility/transport/rfcomm/rfcomm.cpp
===================================================================
--- source/ariba/utility/transport/rfcomm/rfcomm.cpp	(revision 5289)
+++ source/ariba/utility/transport/rfcomm/rfcomm.cpp	(revision 5406)
@@ -11,7 +11,8 @@
 #include <deque>
 
-
 namespace ariba {
 namespace transport {
+
+use_logging_cpp(rfcomm)
 
 using namespace boost::asio;
@@ -31,5 +32,6 @@
 public:
 	link_info(io_service& io ) :
-		up(false), local(), remote(), socket(io), size(0), buffer(NULL), sending(false) {
+		up(false), local(), remote(), socket(io), connect_retries(0),
+		size(0), buffer(NULL), sending(false) {
 	}
 
@@ -38,4 +40,5 @@
 	rfcomm_endpoint local, remote;
 	bluetooth::rfcomm::socket socket;
+	int connect_retries;
 
 	// read buffer
@@ -50,10 +53,9 @@
 };
 
-void rfcomm::remove_info(link_info* info) {
-	for (vector<link_info*>::iterator i = links.begin(); i!=links.end();i++)
-		if (*i==info) {
-			delete info;
-			links.erase(i);
-		}
+void rfcomm::shutdown(link_info* info) {
+	if (info != NULL && info->up) {
+		info->up = false;
+		info->socket.shutdown( bluetooth::rfcomm::socket::shutdown_both );
+	}
 }
 
@@ -76,4 +78,5 @@
 rfcomm::rfcomm(uint16_t channel) :
 	channel(channel), io(asio_io_service::alloc()) {
+	accept_retries = 0;
 }
 
@@ -88,9 +91,11 @@
 
 	// create acceptor
-	cout << "Binding to channel " << channel << endl;
+	logging_info( "Binding to channel " << channel );
 	acceptor = new bluetooth::rfcomm::acceptor(io,
 		bluetooth::rfcomm::endpoint(bluetooth::rfcomm::get(), channel )
 	);
 
+	send_data = new link_data();
+
 	// start accepting
 	start_accept();
@@ -98,8 +103,10 @@
 
 void rfcomm::stop() {
+	logging_info( "Stopping asio rfcomm" );
 
 }
 
 void rfcomm::send(const address_v* remote, const uint8_t* data, size_t size) {
+
 	// get end-point
 	rfcomm_endpoint endpoint = *remote;
@@ -107,15 +114,22 @@
 
 	// try to find established connector
+	logging_debug("Trying to find a already existing link.");
 	link_info* info = NULL;
 	for (size_t i = 0; i < links.size(); i++)
 		if (links[i]->remote.mac() == endpoint.mac()) {
+			logging_debug("Using already established link");
 			info = links[i];
 			break;
 		}
 
-	// not found? ->try to connect
-	if (info==NULL) {
-		cout << "Connecting to " << endpoint.to_string() << endl;
-		info = new link_info(io);
+	// not found, or not up? ->try to (re-)connect
+	if (info==NULL || !info->up) {
+		logging_debug( "Connecting to " << endpoint.to_string() );
+		if (info != NULL && !info->up) {
+			logging_debug("Old link is down. Trying to re-establish link.");
+		} else {
+			info = new link_info(io);
+		}
+		info->remote = endpoint;
 		info->socket.async_connect( convert(endpoint), boost::bind(
 			&rfcomm::handle_connect, this,
@@ -156,5 +170,16 @@
 
 void rfcomm::terminate(const address_v* local, const address_v* remote) {
-	// not supported right now!
+	// get end-point
+	rfcomm_endpoint endpoint = *remote;
+
+	for (size_t i = 0; i < links.size(); i++)
+		if (links[i]->remote.mac() == endpoint.mac()) {
+
+			// close socket
+			links[i]->socket.cancel();
+			links[i]->socket.close();
+			links[i]->up = false;
+			break;
+		}
 }
 
@@ -165,5 +190,5 @@
 void rfcomm::start_accept() {
 
-	cout << "Waiting for connections ..." << endl;
+	logging_info( "Waiting for connections ..." );
 
 	// start accepting a connection
@@ -181,6 +206,13 @@
 void rfcomm::handle_accept(const error_code& error, link_info* info) {
 	if (error) {
-		cout << "Error accepting" << endl;
+		logging_error( "Error waiting for new connections: " << error
+				<< ", trying to recover (attempt " << accept_retries << ")");
 		delete info;
+
+		// restart accepting
+		if (accept_retries<3) {
+			accept_retries++;
+			start_accept();
+		}
 		return;
 	}
@@ -191,5 +223,6 @@
 	info->remote = convert( info->socket.remote_endpoint() );
 
-	cout << "Accepting incoming connection from " << info->remote.to_string() << endl;
+	logging_debug("Accepting incoming connection from "
+		<< info->remote.to_string() );
 
 	// add to list
@@ -208,7 +241,21 @@
 void rfcomm::handle_connect( const error_code& error, link_info* info ) {
 	if (error) {
-		cout << "Error connecting ..." << endl;
-		delete info;
-		return;
+		logging_error( "Can not connect. Retrying ... "
+				"(attempt " << info->connect_retries << ")" );
+
+		// do we retry this connection? yes->
+		if (info->connect_retries<3) {
+			// increase counter
+			info->connect_retries++;
+
+			// retry connection attempt
+			info->socket.async_connect( convert(info->remote), boost::bind(
+				&rfcomm::handle_connect, this,
+				boost::asio::placeholders::error, info
+			));
+
+		} else { // no-> delete link and stop
+			return;
+		}
 	}
 
@@ -218,5 +265,5 @@
 	info->remote = convert( info->socket.remote_endpoint() );
 
-	cout << "Connected to " << info->remote.to_string() << endl;
+	logging_debug( "Connected to " << info->remote.to_string() );
 
 	// add to list
@@ -233,10 +280,14 @@
 	// start read
 	boost::asio::async_read(info->socket,
+
 		// read size of packet
 		boost::asio::buffer(info->size_, 4),
+
 		// bind handler
 		boost::bind(
+
 			// bind parameters
 			&rfcomm::handle_read_header, this,
+
 			// handler parameters
 			placeholders::error, placeholders::bytes_transferred, info
@@ -248,6 +299,13 @@
 	link_info* info) {
 
+	// handle error
+	if (error) {
+		logging_error("Failed to receive message payload.");
+		shutdown(info);
+		return;
+	}
+
 	// ignore errors and wait for all data to be received
-	if (error || bytes != 4) return;
+	if (bytes != 4) return;
 
 	// get size
@@ -255,5 +313,5 @@
 			(info->size_[2]<< 8) + (info->size_[3] << 0);
 
-	cout << "receive message of size " << info->size << endl;
+	logging_debug( "Message header received -> receive message of size " << info->size );
 
 	// allocate buffer
@@ -277,11 +335,15 @@
 	link_info* info) {
 
-	// ignore errors and wait for all data to be received
-	if (error || bytes != info->size) {
-		if (error) remove_info(info);
+	// check error
+	if (error) {
+		logging_error("Failed to receive message payload.");
+		shutdown(info);
 		return;
 	}
 
-	cout << "received message of size " << info->size << endl;
+	// wait for all data to be received
+	if (bytes != info->size) return;
+
+	logging_debug( "Received message of size " << info->size );
 
 	// deliver data
@@ -296,4 +358,6 @@
 
 void rfcomm::start_write( link_info* info ) {
+	boost::mutex::scoped_lock(info->mutex);
+
 	// do not start writing if sending is in progress
 	if (info->sending || !info->up || info->send_buffer.size()==0) return;
@@ -302,12 +366,10 @@
 
 	// safely remove data from deque
-	info->mutex.lock();
-	link_data data = info->send_buffer.front();
+	*send_data = info->send_buffer.front();
 	info->send_buffer.pop_front();
-	info->mutex.unlock();
 
 	boost::array<boost::asio::mutable_buffer, 2> buffer;
-	buffer[0] = boost::asio::buffer(data.size_,4);
-	buffer[1] = boost::asio::buffer(data.buffer,data.size);
+	buffer[0] = boost::asio::buffer(send_data->size_,4);
+	buffer[1] = boost::asio::buffer(send_data->buffer,send_data->size);
 
 	// start writing
@@ -321,5 +383,5 @@
 			// handler parameters
 			placeholders::error, placeholders::bytes_transferred,
-			info, data.size, data.buffer
+			info, send_data->size, send_data->buffer
 		)
 	);
@@ -332,11 +394,13 @@
 	if (error || bytes != (size+4) ) {
 		if (error) {
-			cout << "Message sent error" << endl;
-			remove_info(info);
+			logging_error( "Message sent error" );
+
+			// close socket
+			info->socket.close();
+			info->up = false;
 		}
 		return;
 	}
-
-	cout << "Message sent" << endl;
+	logging_debug( "Message sent" );
 
 	// free buffer
