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