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