plugins/snep.c

   1 /*
   2  *
   3  *  neard - Near Field Communication manager
   4  *
   5  *  Copyright (C) 2012  Intel Corporation. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #include <stdint.h>
  27 #include <errno.h>
  28 #include <string.h>
  29 #include <sys/socket.h>
  30
  31 #include <linux/socket.h>
  32 #include <linux/nfc.h>
  33
  34 #include <near/plugin.h>
  35 #include <near/log.h>
  36 #include <near/types.h>
  37 #include <near/adapter.h>
  38 #include <near/device.h>
  39 #include <near/ndef.h>
  40 #include <near/tlv.h>
  41
  42 #include "p2p.h"
  43
  44 #define SNEP_VERSION     0x10
  45
  46 /* Request codes */
  47 #define SNEP_REQ_CONTINUE 0x00
  48 #define SNEP_REQ_GET      0x01
  49 #define SNEP_REQ_PUT      0x02
  50 #define SNEP_REQ_REJECT   0x7f
  51
  52 /* Response codes */
  53 #define SNEP_RESP_CONTINUE  0x80
  54 #define SNEP_RESP_SUCCESS   0x81
  55 #define SNEP_RESP_NOT_FOUND 0xc0
  56 #define SNEP_RESP_EXCESS    0xc1
  57 #define SNEP_RESP_BAD_REQ   0xc2
  58 #define SNEP_RESP_NOT_IMPL  0xe0
  59 #define SNEP_RESP_VERSION   0xe1
  60 #define SNEP_RESP_REJECT    0xff
  61
  62 #define SNEP_REQ_PUT_HEADER_LENGTH 6
  63 #define SNEP_REQ_GET_HEADER_LENGTH 10
  64 /* TODO: Right now it is dummy, need to get correct value
  65  * from lower layers */
  66 #define SNEP_REQ_MAX_FRAGMENT_LENGTH 128
  67
  68 struct p2p_snep_data {
  69         uint8_t request_code;
  70         uint8_t *nfc_data;
  71         uint32_t nfc_data_length;
  72         uint32_t nfc_data_current_length;
  73         uint8_t *nfc_data_ptr;
  74         uint32_t adapter_idx;
  75         uint32_t target_idx;
  76         gboolean respond_continue;
  77         near_tag_io_cb cb;
  78 };
  79
  80 struct snep_fragment {
  81         uint32_t len;
  82         uint8_t *data;
  83 };
  84
  85 struct p2p_snep_put_req_data {
  86         uint8_t fd;
  87         uint32_t adapter_idx;
  88         uint32_t target_idx;
  89         near_device_io_cb cb;
  90         guint watch;
  91
  92         GSList *fragments;
  93 };
  94
  95 struct p2p_snep_req_frame {
  96         uint8_t version;
  97         uint8_t request;
  98         uint32_t length;
  99         uint8_t ndef[];
 100 } __attribute__((packed));
 101
 102 struct p2p_snep_resp_frame {
 103         uint8_t version;
 104         uint8_t response;
 105         uint32_t length;
 106         uint8_t info[];
 107 } __attribute__((packed));
 108
 109 static GHashTable *snep_client_hash = NULL;
 110
 111 static void free_snep_client(gpointer data)
 112 {
 113         struct p2p_snep_data *snep_data = data;
 114
 115         g_free(snep_data->nfc_data);
 116         g_free(snep_data);
 117 }
 118
 119 static void snep_response_noinfo(int client_fd, uint8_t response)
 120 {
 121         struct p2p_snep_resp_frame resp;
 122
 123         DBG("Response 0x%x", response);
 124
 125         resp.version = SNEP_VERSION;
 126         resp.response = response;
 127         resp.length = 0;
 128
 129         send(client_fd, &resp, sizeof(resp), 0);
 130 }
 131
 132 static void snep_close(int client_fd, int err)
 133 {
 134         struct p2p_snep_data *snep_data;
 135
 136         DBG("");
 137
 138         snep_data = g_hash_table_lookup(snep_client_hash,
 139                                         GINT_TO_POINTER(client_fd));
 140         if (snep_data == NULL)
 141                 return;
 142
 143         snep_data->cb(snep_data->adapter_idx, snep_data->target_idx, err);
 144
 145         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 146 }
 147
 148 static void snep_response_with_info(int client_fd, uint8_t response,
 149                 uint8_t *data, int length)
 150 {
 151         struct p2p_snep_resp_frame *resp;
 152
 153         DBG("Response with info 0x%x (len:%d)", response, length);
 154
 155         resp = g_try_malloc0(sizeof(struct p2p_snep_resp_frame) + length);
 156         if (resp == NULL) {
 157                 DBG("Memory allocation error");
 158                 return;
 159         }
 160
 161         /* Fill */
 162         resp->version = SNEP_VERSION;
 163         resp->response = response;
 164         resp->length = GUINT32_TO_BE(length);
 165         memcpy(resp->info, data, length);
 166
 167         send(client_fd, resp, sizeof(struct p2p_snep_resp_frame) + length, 0);
 168
 169         g_free(resp);
 170 }
 171
 172 /*
 173  * snep_parse_handover_record
 174  *
 175  * The hr frame should be here BUT:
 176  *      The first 4 bytes are the Max Allowed Length
 177  *
 178  * - Because of an Android's BUGs:
 179  *      - the Hr frame is not correct; a Hr record
 180  *      is embedded in a ... Hr record !!! The author
 181  *      used 'Hr' instead of 'cr'
 182  *      - The OOB block is badly written:
 183  *      - the payload ID should be the same in the 'ac' record
 184  *              and the OOB record.
 185  *      - The OOB data length bytes must be swapped (Big endian to Little E.)
 186  *
 187  * The hack fixes the first issue (bluetooth.c fixes the second) !
 188  * */
 189 static void snep_parse_handover_record(int client_fd, uint8_t *ndef,
 190                 uint32_t nfc_data_length)
 191 {
 192         GList *records;
 193         struct near_ndef_message *msg;
 194
 195         if (ndef == NULL)
 196                 return;
 197
 198         /*
 199          * Bugfix Android: Fix 'cr' instead of 'Hr'
 200          * Bug is in Google:HandoverManager.java:645
 201          */
 202         if (strncmp((char *)(ndef + 9), "Hr", 2) == 0)
 203                 *(ndef + 9) = 'c';
 204
 205         /* Parse the incoming frame */
 206         records = near_ndef_parse(ndef, nfc_data_length);
 207
 208         /*
 209          * If we received a Hr, we must build a Hs and send it.
 210          * If the frame is a Hs, nothing more to do (SNEP REPLY is SUCCESS and
 211          * the pairing is done in near_ndef_parse()
 212          * */
 213         if (strncmp((char *)(ndef + 3), "Hr", 2) == 0) {
 214                 msg = near_ndef_prepare_handover_record("Hs", records->data,
 215                                                 NEAR_CARRIER_BLUETOOTH);
 216
 217                 near_info("Send SNEP / Hs frame");
 218                 snep_response_with_info(client_fd, SNEP_RESP_SUCCESS,
 219                                         msg->data, msg->length);
 220                 g_free(msg->data);
 221                 g_free(msg);
 222         }
 223
 224         near_ndef_records_free(records);
 225
 226         return;
 227 }
 228
 229 static near_bool_t snep_read_ndef(int client_fd,
 230                                         struct p2p_snep_data *snep_data)
 231 {
 232         int bytes_recv, remaining_bytes;
 233         struct near_device *device;
 234         GList *records;
 235
 236         DBG("");
 237
 238         remaining_bytes = snep_data->nfc_data_length -
 239                                 snep_data->nfc_data_current_length;
 240
 241         DBG("Remaining bytes %d", remaining_bytes);
 242
 243         bytes_recv = recv(client_fd, snep_data->nfc_data_ptr, remaining_bytes,
 244                                                                 MSG_DONTWAIT);
 245         if (bytes_recv < 0) {
 246                 near_error("%d %s", bytes_recv, strerror(errno));
 247
 248                 /* Some more data should show up */
 249                 if (errno == EAGAIN)
 250                         return TRUE;
 251
 252                 goto out;
 253         }
 254
 255         DBG("Received %d bytes", bytes_recv);
 256
 257         snep_data->nfc_data_current_length += bytes_recv;
 258         snep_data->nfc_data_ptr += bytes_recv;
 259
 260         if (snep_data->nfc_data_length != snep_data->nfc_data_current_length) {
 261                 if (snep_data->respond_continue == FALSE) {
 262                         DBG("Continue");
 263                         snep_data->respond_continue = TRUE;
 264                         snep_response_noinfo(client_fd, SNEP_RESP_CONTINUE);
 265                 }
 266
 267                 return TRUE;
 268         }
 269
 270         if (snep_data->request_code == SNEP_REQ_GET) {
 271                 /*
 272                  * This goes against the SNEP specification:
 273                  * "The default server SHALL NOT accept Get requests." but
 274                  * the first Android Handover implementation (Jelly Bean)
 275                  * does Handover through SNEP via GET frames...Since Android
 276                  * seems popular these days, we'd better support that spec
 277                  * violation.
 278                  *
 279                  * Parse the Hr and send a Hs
 280                  * Max allowed size in the first 4 bytes
 281                  */
 282                 snep_parse_handover_record(client_fd, snep_data->nfc_data + 4,
 283                                 snep_data->nfc_data_length - 4);
 284         } else {
 285                 snep_response_noinfo(client_fd, SNEP_RESP_SUCCESS);
 286                 if (near_device_add_data(snep_data->adapter_idx,
 287                                 snep_data->target_idx,
 288                                 snep_data->nfc_data,
 289                                 snep_data->nfc_data_length) < 0)
 290                         goto out;
 291
 292                 device = near_device_get_device(snep_data->adapter_idx,
 293                                 snep_data->target_idx);
 294                 if (device == NULL)
 295                         goto out;
 296
 297                 records = near_ndef_parse(snep_data->nfc_data,
 298                                 snep_data->nfc_data_length);
 299                 near_device_add_records(device, records, snep_data->cb, 0);
 300         }
 301
 302 out:
 303         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 304
 305         return FALSE;
 306 }
 307
 308 static near_bool_t snep_read(int client_fd,
 309                                 uint32_t adapter_idx, uint32_t target_idx,
 310                                 near_tag_io_cb cb)
 311 {
 312         struct p2p_snep_data *snep_data;
 313         struct p2p_snep_req_frame frame;
 314         int bytes_recv;
 315         uint32_t ndef_length;
 316
 317         DBG("");
 318
 319         snep_data = g_hash_table_lookup(snep_client_hash,
 320                                         GINT_TO_POINTER(client_fd));
 321
 322         /*
 323          * We already got something from this client, we should try
 324          * to continue reading.
 325          */
 326         if (snep_data != NULL)
 327                 return snep_read_ndef(client_fd, snep_data);
 328
 329         /* TODO Try with PEEK */
 330         bytes_recv = recv(client_fd, &frame, sizeof(frame), 0);
 331         if (bytes_recv < 0) {
 332                 near_error("Could not read SNEP frame %d", bytes_recv);
 333                 return bytes_recv;
 334         }
 335
 336         ndef_length = GINT_FROM_BE(frame.length);
 337
 338         DBG("Allocating SNEP data %d", ndef_length);
 339
 340         snep_data = g_try_malloc0(sizeof(struct p2p_snep_data));
 341         if (snep_data == NULL)
 342                 return FALSE;
 343
 344         snep_data->nfc_data = g_try_malloc0(ndef_length + TLV_SIZE);
 345         if (snep_data->nfc_data == NULL) {
 346                 g_free(snep_data);
 347                 return FALSE;
 348         }
 349
 350         snep_data->nfc_data_length = ndef_length;
 351         snep_data->nfc_data_ptr = snep_data->nfc_data;
 352         snep_data->adapter_idx = adapter_idx;
 353         snep_data->target_idx = target_idx;
 354         snep_data->respond_continue = FALSE;
 355         snep_data->cb = cb;
 356
 357         g_hash_table_insert(snep_client_hash,
 358                                         GINT_TO_POINTER(client_fd), snep_data);
 359
 360         snep_data->request_code = frame.request;
 361
 362         DBG("Request 0x%x", frame.request);
 363
 364         switch (frame.request) {
 365         case SNEP_REQ_CONTINUE:
 366                 near_error("Unsupported SNEP request code");
 367                 snep_response_noinfo(client_fd, SNEP_RESP_NOT_IMPL);
 368                 return FALSE;
 369         case SNEP_REQ_GET:
 370         case SNEP_REQ_PUT:
 371                 return snep_read_ndef(client_fd, snep_data);
 372         }
 373
 374         return FALSE;
 375 }
 376
 377 static void free_snep_fragment(gpointer data)
 378 {
 379         struct snep_fragment *fragment = data;
 380
 381         if (fragment != NULL)
 382                 g_free(fragment->data);
 383
 384         g_free(fragment);
 385         fragment = NULL;
 386 }
 387
 388 static void free_snep_push_data(gpointer userdata, int status)
 389 {
 390         struct p2p_snep_put_req_data *data;
 391
 392         DBG("");
 393
 394         data = (struct p2p_snep_put_req_data *) userdata;
 395
 396         close(data->fd);
 397
 398         if (data->cb)
 399                 data->cb(data->adapter_idx, data->target_idx, status);
 400
 401         if (data->watch > 0)
 402                 g_source_remove(data->watch);
 403
 404         g_slist_free_full(data->fragments, free_snep_fragment);
 405         g_free(data);
 406 }
 407
 408 static int snep_send_fragment(struct p2p_snep_put_req_data *req)
 409 {
 410         struct snep_fragment *fragment;
 411         int err;
 412
 413         DBG("");
 414
 415         if (req == NULL || req->fragments == NULL ||
 416                 g_slist_length(req->fragments) == 0)
 417                 return -EINVAL;
 418
 419         fragment = req->fragments->data;
 420
 421         err = send(req->fd, fragment->data, fragment->len, 0);
 422
 423         req->fragments = g_slist_remove(req->fragments, fragment);
 424         g_free(fragment->data);
 425         g_free(fragment);
 426
 427         return err;
 428 }
 429
 430 static int snep_push_response(struct p2p_snep_put_req_data *req)
 431 {
 432         struct p2p_snep_resp_frame frame;
 433         uint8_t *ndef;
 434         uint32_t ndef_len;
 435         int bytes_recv, err;
 436
 437         DBG("");
 438
 439         bytes_recv = recv(req->fd, &frame, sizeof(frame), 0);
 440         if (bytes_recv < 0) {
 441                 near_error("Could not read SNEP frame %d", bytes_recv);
 442                 return bytes_recv;
 443         }
 444
 445         /* Check frame length */
 446         frame.length = g_ntohl(frame.length);
 447
 448         DBG("Response 0x%x", frame.response);
 449
 450         switch (frame.response) {
 451         case SNEP_RESP_CONTINUE:
 452                 while (g_slist_length(req->fragments) != 0) {
 453                         err = snep_send_fragment(req);
 454                         if (err < 0)
 455                                 return err;
 456                 }
 457
 458                 return frame.response;
 459
 460         case SNEP_RESP_SUCCESS:
 461                 if (frame.length == 0)
 462                         return 0;
 463
 464                 /* Get the incoming data */
 465                 ndef_len = frame.length;
 466                 ndef = g_try_malloc0(ndef_len);
 467                 if (ndef == NULL)
 468                         return -ENOMEM;
 469
 470                 bytes_recv = recv(req->fd, ndef, ndef_len, 0);
 471                 if (bytes_recv < 0) {
 472                         near_error("Could not read SNEP frame %d", bytes_recv);
 473                         return bytes_recv;
 474                 }
 475
 476                 /* Not enough bytes */
 477                 if (bytes_recv < 6)
 478                         return -EINVAL;
 479
 480                 if (strncmp((char *)(ndef + 3), "Hs", 2) == 0)
 481                         snep_parse_handover_record(req->fd, ndef, ndef_len);
 482
 483                 g_free(ndef);
 484
 485                 return 0;
 486         }
 487
 488         return -1;
 489 }
 490
 491 static gboolean snep_push_event(GIOChannel *channel,
 492                                 GIOCondition condition, gpointer data)
 493 {
 494         int err;
 495
 496         DBG("condition 0x%x", condition);
 497
 498         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
 499
 500                 near_error("Error with SNEP channel");
 501
 502                 free_snep_push_data(data, -1);
 503
 504                 return FALSE;
 505         }
 506
 507         err = snep_push_response(data);
 508         if (err <= 0) {
 509                 free_snep_push_data(data, err);
 510
 511                 return FALSE;
 512         }
 513
 514         return TRUE;
 515 }
 516
 517 static int snep_push_prepare_fragments(struct p2p_snep_put_req_data *req,
 518                                                 struct near_ndef_message *ndef)
 519 {
 520         struct snep_fragment *fragment;
 521         uint32_t max_fragment_len;
 522
 523         DBG("");
 524
 525         max_fragment_len = SNEP_REQ_MAX_FRAGMENT_LENGTH;
 526
 527         while (ndef->offset < ndef->length) {
 528
 529                 fragment = g_try_malloc0(sizeof(struct snep_fragment));
 530                 if (fragment == NULL)
 531                         return -ENOMEM;
 532
 533                 if (max_fragment_len <= (ndef->length - ndef->offset))
 534                         fragment->len = max_fragment_len;
 535                 else
 536                         fragment->len = ndef->length - ndef->offset;
 537
 538                 fragment->data = g_try_malloc0(fragment->len);
 539                 if (fragment->data == NULL) {
 540                         g_free(fragment);
 541                         return -ENOMEM;
 542                 }
 543
 544                 memcpy(fragment->data, ndef->data + ndef->offset,
 545                                         fragment->len);
 546                 ndef->offset += fragment->len;
 547                 req->fragments = g_slist_append(req->fragments, fragment);
 548         }
 549
 550         return 0;
 551 }
 552
 553 static int snep_push(int fd, uint32_t adapter_idx, uint32_t target_idx,
 554                         struct near_ndef_message *ndef,
 555                         near_device_io_cb cb)
 556 {
 557         struct p2p_snep_put_req_data *req;
 558         struct p2p_snep_req_frame header;
 559         struct snep_fragment *fragment;
 560         uint32_t max_fragment_len;
 561         GIOChannel *channel;
 562         gboolean fragmenting;
 563         int err;
 564         int snep_req_header_length, snep_additional_length;
 565
 566         DBG("");
 567
 568         req = g_try_malloc0(sizeof(struct p2p_snep_put_req_data));
 569         if (req == NULL) {
 570                 err = -ENOMEM;
 571                 goto error;
 572         }
 573
 574         channel = g_io_channel_unix_new(fd);
 575         g_io_channel_set_close_on_unref(channel, TRUE);
 576
 577         req->fd = fd;
 578         req->adapter_idx = adapter_idx;
 579         req->target_idx = target_idx;
 580         req->cb = cb;
 581         ndef->offset = 0;
 582         req->watch = g_io_add_watch(channel, G_IO_IN | G_IO_HUP | G_IO_NVAL |
 583                                         G_IO_ERR, snep_push_event,
 584                                         (gpointer) req);
 585
 586         max_fragment_len = SNEP_REQ_MAX_FRAGMENT_LENGTH;
 587         header.version = SNEP_VERSION;
 588
 589         /* Check if Hr or Hs for Handover over SNEP */
 590         if (*(char *)(ndef->data + 3) == 'H') {
 591                 header.request = SNEP_REQ_GET;          /* Get for android */
 592                 snep_req_header_length = SNEP_REQ_GET_HEADER_LENGTH;
 593                 snep_additional_length = 4;  /* 4 Acceptable Length */
 594         } else {
 595                 header.request = SNEP_REQ_PUT;
 596                 snep_req_header_length = SNEP_REQ_PUT_HEADER_LENGTH;
 597                 snep_additional_length = 0;
 598         }
 599
 600         header.length = GUINT32_TO_BE(ndef->length + snep_additional_length);
 601
 602         fragment = g_try_malloc0(sizeof(struct snep_fragment));
 603         if (fragment == NULL) {
 604                 err = -ENOMEM;
 605                 goto error;
 606         }
 607
 608         if (max_fragment_len >= (ndef->length + snep_req_header_length)) {
 609                 fragment->len = ndef->length + snep_req_header_length;
 610                 fragmenting = FALSE;
 611         } else {
 612                 fragment->len = max_fragment_len;
 613                 fragmenting = TRUE;
 614         }
 615
 616         fragment->data = g_try_malloc0(fragment->len);
 617         if (fragment->data == NULL) {
 618                 g_free(fragment);
 619                 err = ENOMEM;
 620                 goto error;
 621         }
 622
 623         /* Header to data - common header */
 624         memcpy(fragment->data, (uint8_t *)&header, SNEP_REQ_PUT_HEADER_LENGTH);
 625
 626         /* if GET, we add the Acceptable length */
 627         if (header.request == SNEP_REQ_GET)
 628                 *(uint32_t *)(fragment->data + SNEP_REQ_PUT_HEADER_LENGTH)  =
 629                                 GUINT32_TO_BE(snep_req_header_length);
 630
 631         if (fragmenting == TRUE) {
 632                 memcpy(fragment->data + snep_req_header_length, ndef->data,
 633                                 max_fragment_len - snep_req_header_length);
 634                 ndef->offset = max_fragment_len - snep_req_header_length;
 635
 636                 err = snep_push_prepare_fragments(req, ndef);
 637                 if (err < 0) {
 638                         g_free(fragment->data);
 639                         g_free(fragment);
 640                         goto error;
 641                 }
 642
 643         } else {
 644                 memcpy(fragment->data + snep_req_header_length,
 645                                         ndef->data, ndef->length);
 646         }
 647
 648         err = send(fd, fragment->data, fragment->len, 0);
 649         if (err < 0) {
 650                 near_error("Sending failed %d", err);
 651                 g_free(fragment->data);
 652                 g_free(fragment);
 653
 654                 goto error;
 655         }
 656
 657         g_free(fragment->data);
 658         g_free(fragment);
 659
 660         return 0;
 661
 662 error:
 663         free_snep_push_data(req, err);
 664
 665         return err;
 666 }
 667
 668 struct near_p2p_driver snep_driver = {
 669         .name = "SNEP",
 670         .service_name = NEAR_DEVICE_SN_SNEP,
 671         .fallback_service_name = NEAR_DEVICE_SN_NPP,
 672         .read = snep_read,
 673         .push = snep_push,
 674         .close = snep_close,
 675 };
 676
 677 int snep_init(void)
 678 {
 679         snep_client_hash = g_hash_table_new_full(g_direct_hash, g_direct_equal,
 680                                                         NULL, free_snep_client);
 681
 682         return near_p2p_register(&snep_driver);
 683 }
 684
 685 void snep_exit(void)
 686 {
 687         near_p2p_unregister(&snep_driver);
 688
 689         g_hash_table_destroy(snep_client_hash);
 690         snep_client_hash = NULL;
 691 }