00001 #include "rfcomm.hpp"
00002
00003 #include "ariba/utility/transport/asio/asio_io_service.h"
00004 #include "ariba/utility/transport/asio/rfcomm.hpp"
00005 #include "ariba/utility/transport/asio/bluetooth_endpoint.hpp"
00006
00007 #include <boost/asio.hpp>
00008 #include <boost/thread.hpp>
00009 #include <boost/array.hpp>
00010 #include <memory.h>
00011 #include <deque>
00012 #include <cerrno>
00013
00014 namespace ariba {
00015 namespace transport {
00016
00017 use_logging_cpp(rfcomm)
00018
00019 using namespace boost::asio;
00020 using namespace detail;
00021 using namespace std;
00022
00023 using boost::system::error_code;
00024
00025 class link_data {
00026 public:
00027 uint8_t size_[4];
00028 size_t size;
00029 uint8_t* buffer;
00030 };
00031
00032 class link_info {
00033 public:
00034 link_info(io_service& io ) :
00035 io(io), up(false), connecting(false), local(), remote(), socket(new bluetooth::rfcomm::socket(io)), connect_retries(0),
00036 size(0), buffer(NULL), sending(false) {
00037 }
00038
00039 ~link_info() {
00040 delete socket;
00041 }
00042
00043 void reinit() {
00044 delete socket;
00045 socket = new bluetooth::rfcomm::socket(io);
00046 up = false;
00047 }
00048
00049 io_service& io;
00050
00051
00052 bool up;
00053 bool connecting;
00054 rfcomm_endpoint local, remote;
00055 bluetooth::rfcomm::socket* socket;
00056 int connect_retries;
00057
00058
00059 uint8_t size_[4];
00060 size_t size;
00061 uint8_t* buffer;
00062
00063
00064 bool sending;
00065 boost::mutex mutex;
00066 std::deque<link_data> send_buffer;
00067 };
00068
00069 void rfcomm::shutdown(link_info* info) {
00070 if (info != NULL && info->up) {
00071 info->up = false;
00072 info->socket->shutdown( bluetooth::rfcomm::socket::shutdown_both );
00073 }
00074 }
00075
00076
00077 inline bluetooth::rfcomm::endpoint convert( const rfcomm_endpoint& endpoint ) {
00078 return bluetooth::rfcomm::endpoint(
00079 endpoint.mac().bluetooth(), endpoint.channel().value()
00080 );
00081 }
00082
00083 inline rfcomm_endpoint convert( const bluetooth::rfcomm::endpoint& endpoint ) {
00084 mac_address mac;
00085 mac.bluetooth( endpoint.address() );
00086 rfcomm_channel_address channel;
00087 channel.value( endpoint.channel() );
00088 return rfcomm_endpoint( mac, channel );
00089 }
00090
00091
00092 rfcomm::rfcomm(uint16_t channel) :
00093 channel(channel), io(asio_io_service::alloc()) {
00094 accept_retries = 0;
00095 }
00096
00097 rfcomm::~rfcomm() {
00098 asio_io_service::free();
00099 }
00100
00101 void rfcomm::start() {
00102
00103
00104 asio_io_service::start();
00105
00106
00107 logging_info( "Binding to channel " << channel );
00108 acceptor = new bluetooth::rfcomm::acceptor(io,
00109 bluetooth::rfcomm::endpoint(bluetooth::rfcomm::get(), channel )
00110 );
00111
00112 send_data = new link_data();
00113
00114
00115 start_accept();
00116 }
00117
00118 void rfcomm::stop() {
00119 logging_info( "Stopping asio rfcomm" );
00120 }
00121
00122 void rfcomm::send(const address_v* remote, const uint8_t* data, size_t size) {
00123
00124
00125 rfcomm_endpoint endpoint = *remote;
00126 endpoint = convert(convert(endpoint));
00127
00128
00129 logging_debug("Trying to find a already existing link.");
00130 link_info* info = NULL;
00131 for (size_t i = 0; i < links.size(); i++)
00132 if (links[i]->remote.mac() == endpoint.mac()) {
00133 logging_debug("Using already established link");
00134 info = links[i];
00135 break;
00136 }
00137
00138
00139 if (info==NULL || ((!info->up || !info->socket->is_open()) && !info->connecting) ) {
00140 logging_debug( "Connecting to " << endpoint.to_string() );
00141 if (info != NULL && (!info->up || !info->socket->is_open())) {
00142 logging_error("Old link is down. Trying to re-establish link.");
00143 info->reinit();
00144 } else {
00145 info = new link_info(io);
00146 }
00147 info->connect_retries = 0;
00148 info->remote = endpoint;
00149 info->connecting = true;
00150 info->socket->async_connect( convert(endpoint), boost::bind(
00151 &rfcomm::handle_connect, this,
00152 boost::asio::placeholders::error, info
00153 ));
00154 links.push_back(info);
00155 }
00156
00157
00158 link_data ldata;
00159 ldata.size = size;
00160 ldata.size_[0] = (size >> 24) & 0xFF;
00161 ldata.size_[1] = (size >> 16) & 0xFF;
00162 ldata.size_[2] = (size >> 8) & 0xFF;
00163 ldata.size_[3] = (size >> 0) & 0xFF;
00164 ldata.buffer = new uint8_t[size];
00165 memcpy(ldata.buffer, data, size);
00166
00167
00168 info->mutex.lock();
00169 info->send_buffer.push_back(ldata);
00170 info->mutex.unlock();
00171
00172
00173 io.post( boost::bind( &rfcomm::start_write, this, info) );
00174 }
00175
00176 void rfcomm::send(const endpoint_set& endpoints, const uint8_t* data, size_t size) {
00177
00178 BOOST_FOREACH( const mac_address mac, endpoints.bluetooth ) {
00179 BOOST_FOREACH( const rfcomm_channel_address channel, endpoints.rfcomm ) {
00180 rfcomm_endpoint endpoint(mac, channel);
00181 address_vf vf = endpoint;
00182 send(vf,data,size);
00183 }
00184 }
00185 }
00186
00187 void rfcomm::terminate( const address_v* remote) {
00188
00189 rfcomm_endpoint endpoint = *remote;
00190
00191 for (size_t i = 0; i < links.size(); i++)
00192 if (links[i]->remote.mac() == endpoint.mac()) {
00193
00194 shutdown(links[i]);
00195 break;
00196 }
00197 }
00198
00199 void rfcomm::register_listener(transport_listener* listener) {
00200 this->listener = listener;
00201 }
00202
00203 void rfcomm::start_accept() {
00204
00205 logging_info( "Waiting for connections ..." );
00206
00207
00208 link_info* info = new link_info(io);
00209 acceptor->async_accept(*info->socket, boost::bind(
00210
00211 &rfcomm::handle_accept, this,
00212
00213
00214 boost::asio::placeholders::error, info
00215 ));
00216 asio_io_service::start();
00217 }
00218
00219 void rfcomm::handle_accept(const error_code& error, link_info* info) {
00220 if (error) {
00221 logging_error( "Error waiting for new connections. Error code: "<< error.message()
00222 << ", trying to recover (attempt " << accept_retries << ")");
00223
00224
00225 if (accept_retries<3) {
00226 accept_retries++;
00227 start_accept();
00228 } else
00229 delete info;
00230
00231 return;
00232 }
00233
00234 links_mutex.lock();
00235
00236
00237 info->up = true;
00238 info->local = convert( info->socket->local_endpoint() );
00239 info->remote = convert( info->socket->remote_endpoint() );
00240
00241 logging_debug("Accepted incoming connection from "
00242 << info->remote.to_string() );
00243
00244
00245 links.push_back(info);
00246 links_mutex.unlock();
00247
00248
00249 start_read(info);
00250 start_write(info);
00251
00252
00253 start_accept();
00254 }
00255
00256 void rfcomm::handle_connect( const error_code& error, link_info* info ) {
00257 if (error) {
00258 logging_error( "Can not connect. Error code: "
00259 << error.message() << " Retrying ... "
00260 "(attempt " << info->connect_retries << ")" );
00261
00262
00263 if (info->connect_retries<3) {
00264
00265 info->connecting = false;
00266 info->connect_retries++;
00267 info->reinit();
00268
00269
00270 info->socket->async_connect( convert(info->remote), boost::bind(
00271 &rfcomm::handle_connect, this,
00272 boost::asio::placeholders::error, info
00273 ));
00274
00275 } else {
00276 return;
00277 }
00278 }
00279
00280
00281 info->up = true;
00282 info->connecting = false;
00283 info->local = convert( info->socket->local_endpoint() );
00284 info->remote = convert( info->socket->remote_endpoint() );
00285
00286 logging_debug( "Connected to " << info->remote.to_string() );
00287
00288
00289 links_mutex.lock();
00290 links.push_back(info);
00291 links_mutex.unlock();
00292
00293
00294 start_read(info);
00295 start_write(info);
00296 }
00297
00298 void rfcomm::start_read(link_info* info) {
00299 logging_debug("Start reading ...");
00300
00301
00302 boost::asio::async_read(*info->socket,
00303
00304
00305 boost::asio::buffer(info->size_, 4),
00306
00307
00308 boost::bind(
00309
00310
00311 &rfcomm::handle_read_header, this,
00312
00313
00314 placeholders::error, placeholders::bytes_transferred, info
00315 )
00316 );
00317 }
00318
00319 void rfcomm::handle_read_header(const error_code& error, size_t bytes,
00320 link_info* info) {
00321
00322
00323 if (error) {
00324 logging_error("Failed to receive message payload. Error code: "
00325 << error.message() );
00326 shutdown(info);
00327 return;
00328 }
00329
00330
00331 if (bytes != 4) return;
00332
00333
00334 info->size = (info->size_[0]<<24) + (info->size_[1] << 16) +
00335 (info->size_[2]<< 8) + (info->size_[3] << 0);
00336
00337
00338 info->buffer = new uint8_t[info->size];
00339
00340
00341 boost::asio::async_read(*info->socket,
00342
00343 boost::asio::buffer(info->buffer, info->size),
00344
00345 boost::bind(
00346
00347 &rfcomm::handle_read_data, this,
00348
00349 placeholders::error, placeholders::bytes_transferred, info
00350 )
00351 );
00352 }
00353
00354 void rfcomm::handle_read_data(const error_code& error, size_t bytes,
00355 link_info* info) {
00356
00357
00358 if (error) {
00359 logging_error("Failed to receive message payload. Error: " << error.message() );
00360 shutdown(info);
00361 return;
00362 }
00363
00364
00365 if (bytes != info->size)
00366 return;
00367
00368
00369 listener->receive_message(this, info->local, info->remote, info->buffer, info->size );
00370
00371
00372 delete [] info->buffer;
00373 for (size_t i=0; i<4; i++) info->size_[i] = 0;
00374
00375 start_read(info);
00376 }
00377
00378 void rfcomm::start_write( link_info* info ) {
00379
00380 if (info->sending || !info->up || info->send_buffer.size()==0) return;
00381
00382
00383 info->sending = true;
00384
00385
00386 *send_data = info->send_buffer.front();
00387 info->send_buffer.pop_front();
00388
00389 boost::array<boost::asio::mutable_buffer, 2> buffer;
00390 buffer[0] = boost::asio::buffer(send_data->size_,4);
00391 buffer[1] = boost::asio::buffer(send_data->buffer,send_data->size);
00392
00393
00394 boost::asio::async_write(*info->socket,
00395
00396 buffer,
00397
00398 boost::bind(
00399
00400 &rfcomm::handle_write_data, this,
00401
00402 placeholders::error, placeholders::bytes_transferred,
00403 info, send_data->size, send_data->buffer
00404 )
00405 );
00406 }
00407
00408 void rfcomm::handle_write_data(const error_code& error, size_t bytes,
00409 link_info* info, size_t size, uint8_t* buffer ) {
00410
00411
00412 if (error) {
00413 logging_error( "Message sent error. Error: " << error.message() );
00414 info->sending = false;
00415 shutdown(info);
00416 return;
00417 }
00418
00419
00420 if (bytes != (size+4) )
00421 return;
00422
00423 logging_debug( "Message sent" );
00424
00425
00426 delete [] buffer;
00427 info->sending = false;
00428
00429
00430 start_write(info);
00431 }
00432
00433 }}