[7464] | 1 | #include "ariba/config.h"
|
---|
[7041] | 2 |
|
---|
| 3 | #ifdef HAVE_LIBBLUETOOTH
|
---|
| 4 |
|
---|
[5284] | 5 | #include "rfcomm.hpp"
|
---|
| 6 |
|
---|
[5993] | 7 | #include "ariba/utility/transport/asio/asio_io_service.h"
|
---|
| 8 | #include "ariba/utility/transport/asio/rfcomm.hpp"
|
---|
| 9 | #include "ariba/utility/transport/asio/bluetooth_endpoint.hpp"
|
---|
[5284] | 10 |
|
---|
| 11 | #include <boost/asio.hpp>
|
---|
| 12 | #include <boost/thread.hpp>
|
---|
| 13 | #include <boost/array.hpp>
|
---|
| 14 | #include <memory.h>
|
---|
| 15 | #include <deque>
|
---|
[5418] | 16 | #include <cerrno>
|
---|
[5284] | 17 |
|
---|
| 18 | namespace ariba {
|
---|
| 19 | namespace transport {
|
---|
| 20 |
|
---|
[5406] | 21 | use_logging_cpp(rfcomm)
|
---|
| 22 |
|
---|
[5284] | 23 | using namespace boost::asio;
|
---|
| 24 | using namespace detail;
|
---|
| 25 | using namespace std;
|
---|
| 26 |
|
---|
| 27 | using boost::system::error_code;
|
---|
| 28 |
|
---|
| 29 | class link_data {
|
---|
| 30 | public:
|
---|
| 31 | uint8_t size_[4];
|
---|
| 32 | size_t size;
|
---|
| 33 | uint8_t* buffer;
|
---|
| 34 | };
|
---|
| 35 |
|
---|
| 36 | class link_info {
|
---|
| 37 | public:
|
---|
| 38 | link_info(io_service& io ) :
|
---|
[5627] | 39 | io(io), up(false), connecting(false), local(), remote(), socket(new bluetooth::rfcomm::socket(io)), connect_retries(0),
|
---|
[5406] | 40 | size(0), buffer(NULL), sending(false) {
|
---|
[5284] | 41 | }
|
---|
| 42 |
|
---|
[5424] | 43 | ~link_info() {
|
---|
| 44 | delete socket;
|
---|
| 45 | }
|
---|
| 46 |
|
---|
| 47 | void reinit() {
|
---|
| 48 | delete socket;
|
---|
| 49 | socket = new bluetooth::rfcomm::socket(io);
|
---|
| 50 | up = false;
|
---|
| 51 | }
|
---|
| 52 |
|
---|
| 53 | io_service& io;
|
---|
| 54 |
|
---|
[5284] | 55 | // state
|
---|
| 56 | bool up;
|
---|
[5627] | 57 | bool connecting;
|
---|
[5284] | 58 | rfcomm_endpoint local, remote;
|
---|
[5424] | 59 | bluetooth::rfcomm::socket* socket;
|
---|
[5406] | 60 | int connect_retries;
|
---|
[5284] | 61 |
|
---|
| 62 | // read buffer
|
---|
| 63 | uint8_t size_[4];
|
---|
| 64 | size_t size;
|
---|
| 65 | uint8_t* buffer;
|
---|
| 66 |
|
---|
| 67 | // send buffer
|
---|
| 68 | bool sending;
|
---|
| 69 | boost::mutex mutex;
|
---|
| 70 | std::deque<link_data> send_buffer;
|
---|
| 71 | };
|
---|
| 72 |
|
---|
[5406] | 73 | void rfcomm::shutdown(link_info* info) {
|
---|
| 74 | if (info != NULL && info->up) {
|
---|
| 75 | info->up = false;
|
---|
[5424] | 76 | info->socket->shutdown( bluetooth::rfcomm::socket::shutdown_both );
|
---|
[5406] | 77 | }
|
---|
[5289] | 78 | }
|
---|
| 79 |
|
---|
| 80 |
|
---|
[5284] | 81 | inline bluetooth::rfcomm::endpoint convert( const rfcomm_endpoint& endpoint ) {
|
---|
| 82 | return bluetooth::rfcomm::endpoint(
|
---|
| 83 | endpoint.mac().bluetooth(), endpoint.channel().value()
|
---|
| 84 | );
|
---|
| 85 | }
|
---|
| 86 |
|
---|
| 87 | inline rfcomm_endpoint convert( const bluetooth::rfcomm::endpoint& endpoint ) {
|
---|
| 88 | mac_address mac;
|
---|
| 89 | mac.bluetooth( endpoint.address() );
|
---|
| 90 | rfcomm_channel_address channel;
|
---|
| 91 | channel.value( endpoint.channel() );
|
---|
| 92 | return rfcomm_endpoint( mac, channel );
|
---|
| 93 | }
|
---|
| 94 |
|
---|
| 95 |
|
---|
| 96 | rfcomm::rfcomm(uint16_t channel) :
|
---|
| 97 | channel(channel), io(asio_io_service::alloc()) {
|
---|
[5406] | 98 | accept_retries = 0;
|
---|
[5284] | 99 | }
|
---|
| 100 |
|
---|
| 101 | rfcomm::~rfcomm() {
|
---|
| 102 | asio_io_service::free();
|
---|
| 103 | }
|
---|
| 104 |
|
---|
| 105 | void rfcomm::start() {
|
---|
| 106 |
|
---|
| 107 | // start io service
|
---|
| 108 | asio_io_service::start();
|
---|
| 109 |
|
---|
| 110 | // create acceptor
|
---|
[5406] | 111 | logging_info( "Binding to channel " << channel );
|
---|
[5284] | 112 | acceptor = new bluetooth::rfcomm::acceptor(io,
|
---|
| 113 | bluetooth::rfcomm::endpoint(bluetooth::rfcomm::get(), channel )
|
---|
| 114 | );
|
---|
| 115 |
|
---|
[5406] | 116 | send_data = new link_data();
|
---|
| 117 |
|
---|
[5284] | 118 | // start accepting
|
---|
| 119 | start_accept();
|
---|
| 120 | }
|
---|
| 121 |
|
---|
| 122 | void rfcomm::stop() {
|
---|
[5406] | 123 | logging_info( "Stopping asio rfcomm" );
|
---|
[5284] | 124 | }
|
---|
| 125 |
|
---|
| 126 | void rfcomm::send(const address_v* remote, const uint8_t* data, size_t size) {
|
---|
[5406] | 127 |
|
---|
[5284] | 128 | // get end-point
|
---|
| 129 | rfcomm_endpoint endpoint = *remote;
|
---|
| 130 | endpoint = convert(convert(endpoint));
|
---|
| 131 |
|
---|
| 132 | // try to find established connector
|
---|
[5406] | 133 | logging_debug("Trying to find a already existing link.");
|
---|
[5284] | 134 | link_info* info = NULL;
|
---|
| 135 | for (size_t i = 0; i < links.size(); i++)
|
---|
| 136 | if (links[i]->remote.mac() == endpoint.mac()) {
|
---|
[5406] | 137 | logging_debug("Using already established link");
|
---|
[5284] | 138 | info = links[i];
|
---|
| 139 | break;
|
---|
| 140 | }
|
---|
| 141 |
|
---|
[5406] | 142 | // not found, or not up? ->try to (re-)connect
|
---|
[5627] | 143 | if (info==NULL || ((!info->up || !info->socket->is_open()) && !info->connecting) ) {
|
---|
[5406] | 144 | logging_debug( "Connecting to " << endpoint.to_string() );
|
---|
[5424] | 145 | if (info != NULL && (!info->up || !info->socket->is_open())) {
|
---|
[5435] | 146 | logging_error("Old link is down. Trying to re-establish link.");
|
---|
[5424] | 147 | info->reinit();
|
---|
[5406] | 148 | } else {
|
---|
| 149 | info = new link_info(io);
|
---|
| 150 | }
|
---|
[5423] | 151 | info->connect_retries = 0;
|
---|
[5406] | 152 | info->remote = endpoint;
|
---|
[5627] | 153 | info->connecting = true;
|
---|
[5424] | 154 | info->socket->async_connect( convert(endpoint), boost::bind(
|
---|
[5284] | 155 | &rfcomm::handle_connect, this,
|
---|
| 156 | boost::asio::placeholders::error, info
|
---|
| 157 | ));
|
---|
[5627] | 158 | links.push_back(info);
|
---|
[5284] | 159 | }
|
---|
| 160 |
|
---|
| 161 | // copy message
|
---|
| 162 | link_data ldata;
|
---|
| 163 | ldata.size = size;
|
---|
| 164 | ldata.size_[0] = (size >> 24) & 0xFF;
|
---|
| 165 | ldata.size_[1] = (size >> 16) & 0xFF;
|
---|
| 166 | ldata.size_[2] = (size >> 8) & 0xFF;
|
---|
| 167 | ldata.size_[3] = (size >> 0) & 0xFF;
|
---|
| 168 | ldata.buffer = new uint8_t[size];
|
---|
| 169 | memcpy(ldata.buffer, data, size);
|
---|
| 170 |
|
---|
| 171 | // enqueue message
|
---|
| 172 | info->mutex.lock();
|
---|
| 173 | info->send_buffer.push_back(ldata);
|
---|
| 174 | info->mutex.unlock();
|
---|
| 175 |
|
---|
| 176 | // start writing
|
---|
[5444] | 177 | io.post( boost::bind( &rfcomm::start_write, this, info) );
|
---|
[5284] | 178 | }
|
---|
| 179 |
|
---|
| 180 | void rfcomm::send(const endpoint_set& endpoints, const uint8_t* data, size_t size) {
|
---|
| 181 | // send a message to each combination of mac-address and channel
|
---|
| 182 | BOOST_FOREACH( const mac_address mac, endpoints.bluetooth ) {
|
---|
| 183 | BOOST_FOREACH( const rfcomm_channel_address channel, endpoints.rfcomm ) {
|
---|
| 184 | rfcomm_endpoint endpoint(mac, channel);
|
---|
| 185 | address_vf vf = endpoint;
|
---|
| 186 | send(vf,data,size);
|
---|
| 187 | }
|
---|
| 188 | }
|
---|
| 189 | }
|
---|
| 190 |
|
---|
[5614] | 191 | void rfcomm::terminate( const address_v* remote) {
|
---|
[5406] | 192 | // get end-point
|
---|
| 193 | rfcomm_endpoint endpoint = *remote;
|
---|
| 194 |
|
---|
| 195 | for (size_t i = 0; i < links.size(); i++)
|
---|
| 196 | if (links[i]->remote.mac() == endpoint.mac()) {
|
---|
| 197 | // close socket
|
---|
[5410] | 198 | shutdown(links[i]);
|
---|
[5406] | 199 | break;
|
---|
| 200 | }
|
---|
[5284] | 201 | }
|
---|
| 202 |
|
---|
| 203 | void rfcomm::register_listener(transport_listener* listener) {
|
---|
| 204 | this->listener = listener;
|
---|
| 205 | }
|
---|
| 206 |
|
---|
| 207 | void rfcomm::start_accept() {
|
---|
| 208 |
|
---|
[5406] | 209 | logging_info( "Waiting for connections ..." );
|
---|
[5284] | 210 |
|
---|
| 211 | // start accepting a connection
|
---|
| 212 | link_info* info = new link_info(io);
|
---|
[5424] | 213 | acceptor->async_accept(*info->socket, boost::bind(
|
---|
[5284] | 214 | // bind parameters
|
---|
| 215 | &rfcomm::handle_accept, this,
|
---|
| 216 |
|
---|
| 217 | // handler parameters
|
---|
| 218 | boost::asio::placeholders::error, info
|
---|
| 219 | ));
|
---|
| 220 | asio_io_service::start();
|
---|
| 221 | }
|
---|
| 222 |
|
---|
| 223 | void rfcomm::handle_accept(const error_code& error, link_info* info) {
|
---|
| 224 | if (error) {
|
---|
[5422] | 225 | logging_error( "Error waiting for new connections. Error code: "<< error.message()
|
---|
[5406] | 226 | << ", trying to recover (attempt " << accept_retries << ")");
|
---|
| 227 |
|
---|
| 228 | // restart accepting
|
---|
| 229 | if (accept_retries<3) {
|
---|
| 230 | accept_retries++;
|
---|
| 231 | start_accept();
|
---|
[5410] | 232 | } else
|
---|
| 233 | delete info;
|
---|
| 234 |
|
---|
[5284] | 235 | return;
|
---|
| 236 | }
|
---|
| 237 |
|
---|
[5423] | 238 | links_mutex.lock();
|
---|
| 239 |
|
---|
[5284] | 240 | // convert endpoints
|
---|
| 241 | info->up = true;
|
---|
[5424] | 242 | info->local = convert( info->socket->local_endpoint() );
|
---|
| 243 | info->remote = convert( info->socket->remote_endpoint() );
|
---|
[5284] | 244 |
|
---|
[5423] | 245 | logging_debug("Accepted incoming connection from "
|
---|
[5406] | 246 | << info->remote.to_string() );
|
---|
[5284] | 247 |
|
---|
| 248 | // add to list
|
---|
| 249 | links.push_back(info);
|
---|
| 250 | links_mutex.unlock();
|
---|
| 251 |
|
---|
| 252 | // start i/o
|
---|
| 253 | start_read(info);
|
---|
| 254 | start_write(info);
|
---|
| 255 |
|
---|
| 256 | // restart accept
|
---|
| 257 | start_accept();
|
---|
| 258 | }
|
---|
| 259 |
|
---|
| 260 | void rfcomm::handle_connect( const error_code& error, link_info* info ) {
|
---|
| 261 | if (error) {
|
---|
[5422] | 262 | logging_error( "Can not connect. Error code: "
|
---|
| 263 | << error.message() << " Retrying ... "
|
---|
[5406] | 264 | "(attempt " << info->connect_retries << ")" );
|
---|
| 265 |
|
---|
| 266 | // do we retry this connection? yes->
|
---|
| 267 | if (info->connect_retries<3) {
|
---|
| 268 | // increase counter
|
---|
[5627] | 269 | info->connecting = false;
|
---|
[5406] | 270 | info->connect_retries++;
|
---|
[5424] | 271 | info->reinit();
|
---|
[5406] | 272 |
|
---|
| 273 | // retry connection attempt
|
---|
[5424] | 274 | info->socket->async_connect( convert(info->remote), boost::bind(
|
---|
[5406] | 275 | &rfcomm::handle_connect, this,
|
---|
| 276 | boost::asio::placeholders::error, info
|
---|
| 277 | ));
|
---|
| 278 |
|
---|
| 279 | } else { // no-> delete link and stop
|
---|
| 280 | return;
|
---|
| 281 | }
|
---|
[5284] | 282 | }
|
---|
| 283 |
|
---|
| 284 | // convert endpoints
|
---|
| 285 | info->up = true;
|
---|
[5627] | 286 | info->connecting = false;
|
---|
[5424] | 287 | info->local = convert( info->socket->local_endpoint() );
|
---|
| 288 | info->remote = convert( info->socket->remote_endpoint() );
|
---|
[5284] | 289 |
|
---|
[5406] | 290 | logging_debug( "Connected to " << info->remote.to_string() );
|
---|
[5284] | 291 |
|
---|
| 292 | // add to list
|
---|
| 293 | links_mutex.lock();
|
---|
| 294 | links.push_back(info);
|
---|
| 295 | links_mutex.unlock();
|
---|
| 296 |
|
---|
| 297 | // start i/o
|
---|
| 298 | start_read(info);
|
---|
| 299 | start_write(info);
|
---|
| 300 | }
|
---|
| 301 |
|
---|
| 302 | void rfcomm::start_read(link_info* info) {
|
---|
[5444] | 303 | logging_debug("Start reading ...");
|
---|
| 304 |
|
---|
[5284] | 305 | // start read
|
---|
[5424] | 306 | boost::asio::async_read(*info->socket,
|
---|
[5406] | 307 |
|
---|
[5284] | 308 | // read size of packet
|
---|
| 309 | boost::asio::buffer(info->size_, 4),
|
---|
[5406] | 310 |
|
---|
[5284] | 311 | // bind handler
|
---|
| 312 | boost::bind(
|
---|
[5406] | 313 |
|
---|
[5284] | 314 | // bind parameters
|
---|
| 315 | &rfcomm::handle_read_header, this,
|
---|
[5406] | 316 |
|
---|
[5284] | 317 | // handler parameters
|
---|
| 318 | placeholders::error, placeholders::bytes_transferred, info
|
---|
| 319 | )
|
---|
| 320 | );
|
---|
| 321 | }
|
---|
| 322 |
|
---|
| 323 | void rfcomm::handle_read_header(const error_code& error, size_t bytes,
|
---|
| 324 | link_info* info) {
|
---|
| 325 |
|
---|
[5406] | 326 | // handle error
|
---|
| 327 | if (error) {
|
---|
[5422] | 328 | logging_error("Failed to receive message payload. Error code: "
|
---|
| 329 | << error.message() );
|
---|
[5406] | 330 | shutdown(info);
|
---|
| 331 | return;
|
---|
| 332 | }
|
---|
| 333 |
|
---|
[5284] | 334 | // ignore errors and wait for all data to be received
|
---|
[5406] | 335 | if (bytes != 4) return;
|
---|
[5284] | 336 |
|
---|
| 337 | // get size
|
---|
| 338 | info->size = (info->size_[0]<<24) + (info->size_[1] << 16) +
|
---|
| 339 | (info->size_[2]<< 8) + (info->size_[3] << 0);
|
---|
| 340 |
|
---|
| 341 | // allocate buffer
|
---|
| 342 | info->buffer = new uint8_t[info->size];
|
---|
| 343 |
|
---|
| 344 | // start read
|
---|
[5424] | 345 | boost::asio::async_read(*info->socket,
|
---|
[5284] | 346 | // read size of packet
|
---|
| 347 | boost::asio::buffer(info->buffer, info->size),
|
---|
| 348 | // bind handler
|
---|
| 349 | boost::bind(
|
---|
| 350 | // bind parameters
|
---|
| 351 | &rfcomm::handle_read_data, this,
|
---|
| 352 | // handler parameters
|
---|
| 353 | placeholders::error, placeholders::bytes_transferred, info
|
---|
| 354 | )
|
---|
| 355 | );
|
---|
| 356 | }
|
---|
| 357 |
|
---|
| 358 | void rfcomm::handle_read_data(const error_code& error, size_t bytes,
|
---|
| 359 | link_info* info) {
|
---|
| 360 |
|
---|
[5406] | 361 | // check error
|
---|
| 362 | if (error) {
|
---|
[5481] | 363 | logging_error("Failed to receive message payload. Error: " << error.message() );
|
---|
[5406] | 364 | shutdown(info);
|
---|
[5289] | 365 | return;
|
---|
| 366 | }
|
---|
[5284] | 367 |
|
---|
[5406] | 368 | // wait for all data to be received
|
---|
[5410] | 369 | if (bytes != info->size)
|
---|
| 370 | return;
|
---|
[5284] | 371 |
|
---|
| 372 | // deliver data
|
---|
| 373 | listener->receive_message(this, info->local, info->remote, info->buffer, info->size );
|
---|
| 374 |
|
---|
| 375 | // free buffers and reset size buffer
|
---|
| 376 | delete [] info->buffer;
|
---|
| 377 | for (size_t i=0; i<4; i++) info->size_[i] = 0;
|
---|
[5286] | 378 |
|
---|
| 379 | start_read(info);
|
---|
[5284] | 380 | }
|
---|
| 381 |
|
---|
| 382 | void rfcomm::start_write( link_info* info ) {
|
---|
| 383 | // do not start writing if sending is in progress
|
---|
| 384 | if (info->sending || !info->up || info->send_buffer.size()==0) return;
|
---|
| 385 |
|
---|
[5481] | 386 | // set sending flag
|
---|
[5428] | 387 | info->sending = true;
|
---|
[5284] | 388 |
|
---|
| 389 | // safely remove data from deque
|
---|
[5406] | 390 | *send_data = info->send_buffer.front();
|
---|
[5284] | 391 | info->send_buffer.pop_front();
|
---|
| 392 |
|
---|
| 393 | boost::array<boost::asio::mutable_buffer, 2> buffer;
|
---|
[5406] | 394 | buffer[0] = boost::asio::buffer(send_data->size_,4);
|
---|
| 395 | buffer[1] = boost::asio::buffer(send_data->buffer,send_data->size);
|
---|
[5284] | 396 |
|
---|
| 397 | // start writing
|
---|
[5424] | 398 | boost::asio::async_write(*info->socket,
|
---|
[5284] | 399 | // read size of packet
|
---|
| 400 | buffer,
|
---|
| 401 | // bind handler
|
---|
| 402 | boost::bind(
|
---|
| 403 | // bind parameters
|
---|
| 404 | &rfcomm::handle_write_data, this,
|
---|
| 405 | // handler parameters
|
---|
| 406 | placeholders::error, placeholders::bytes_transferred,
|
---|
[5406] | 407 | info, send_data->size, send_data->buffer
|
---|
[5284] | 408 | )
|
---|
| 409 | );
|
---|
| 410 | }
|
---|
| 411 |
|
---|
| 412 | void rfcomm::handle_write_data(const error_code& error, size_t bytes,
|
---|
| 413 | link_info* info, size_t size, uint8_t* buffer ) {
|
---|
| 414 |
|
---|
[5410] | 415 | // handle error
|
---|
| 416 | if (error) {
|
---|
[5481] | 417 | logging_error( "Message sent error. Error: " << error.message() );
|
---|
[5428] | 418 | info->sending = false;
|
---|
[5410] | 419 | shutdown(info);
|
---|
| 420 | return;
|
---|
| 421 | }
|
---|
[5406] | 422 |
|
---|
[5410] | 423 | // wait for all data to be sent
|
---|
| 424 | if (bytes != (size+4) )
|
---|
[5284] | 425 | return;
|
---|
[5410] | 426 |
|
---|
[5406] | 427 | logging_debug( "Message sent" );
|
---|
[5284] | 428 |
|
---|
| 429 | // free buffer
|
---|
| 430 | delete [] buffer;
|
---|
[5428] | 431 | info->sending = false;
|
---|
[5284] | 432 |
|
---|
| 433 | // restart-write
|
---|
| 434 | start_write(info);
|
---|
| 435 | }
|
---|
| 436 |
|
---|
| 437 | }} // namespace ariba::transport
|
---|
[7041] | 438 |
|
---|
| 439 | #endif
|
---|