src/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
  33 #include <near/nfc_copy.h>
  34 #include <near/plugin.h>
  35 #include <near/types.h>
  36 #include <near/adapter.h>
  37 #include <near/device.h>
  38 #include <near/ndef.h>
  39 #include <near/tlv.h>
  40 #include <near/snep.h>
  41
  42 #include "near.h"
  43
  44 struct snep_fragment {
  45         uint32_t len;
  46         uint8_t *data;
  47 };
  48
  49 struct p2p_snep_put_req_data {
  50         uint8_t fd;
  51         uint32_t adapter_idx;
  52         uint32_t target_idx;
  53         near_device_io_cb cb;
  54         guint watch;
  55
  56         GSList *fragments;
  57 };
  58
  59 struct p2p_snep_req_frame {
  60         uint8_t version;
  61         uint8_t request;
  62         uint32_t length;
  63         uint8_t ndef[];
  64 } __attribute__((packed));
  65
  66 struct p2p_snep_resp_frame {
  67         uint8_t version;
  68         uint8_t response;
  69         uint32_t length;
  70         uint8_t info[];
  71 } __attribute__((packed));
  72
  73 static GHashTable *snep_client_hash;
  74
  75 /* Callback: free snep data */
  76 static void free_snep_core_client(gpointer data)
  77 {
  78         struct p2p_snep_data *snep_data = data;
  79
  80         DBG("");
  81
  82         g_free(snep_data->nfc_data);
  83         g_free(snep_data);
  84 }
  85
  86 /* Send a short response code */
  87 void near_snep_core_response_noinfo(int client_fd, uint8_t response)
  88 {
  89         struct p2p_snep_resp_frame resp;
  90
  91         DBG("Response 0x%x", response);
  92
  93         resp.version = NEAR_SNEP_VERSION;
  94         resp.response = response;
  95         resp.length = 0;
  96
  97         send(client_fd, &resp, sizeof(resp), 0);
  98 }
  99
 100 /*
 101  * near_snep_core_parse_handover_record
 102  *
 103  * The hr frame should be here BUT:
 104  *      The first 4 bytes are the Max Allowed Length
 105  *
 106  * - Because of an Android's BUGs:
 107  *      - the Hr frame is not correct; a Hr record
 108  *      is embedded in a ... Hr record !!! The author
 109  *      used 'Hr' instead of 'cr'
 110  *      - The OOB block is badly written:
 111  *      - the payload ID should be the same in the 'ac' record
 112  *              and the OOB record.
 113  *      - The OOB data length bytes must be swapped (Big endian to Little E.)
 114  *
 115  * The hack fixes the first issue (bluetooth.c fixes the second) !
 116  * */
 117 void near_snep_core_parse_handover_record(int client_fd, uint8_t *ndef,
 118                 uint32_t nfc_data_length)
 119 {
 120         GList *records;
 121         struct near_ndef_message *msg = NULL;
 122
 123         if (!ndef)
 124                 return;
 125
 126         /*
 127          * Bugfix Android: Fix 'cr' instead of 'Hr'
 128          * Bug is in Google:HandoverManager.java:645
 129          */
 130         if (nfc_data_length > 9 && strncmp((char *)(ndef + 9), "Hr", 2) == 0) {
 131                 DBG("Android 4.1.1 found !!!");
 132                 *(ndef + 9) = 'c';
 133         }
 134
 135         /* Parse the incoming frame */
 136         records = near_ndef_parse_msg(ndef, nfc_data_length, &msg);
 137         if (!records)
 138                 return;
 139
 140         near_ndef_records_free(records);
 141
 142         if (!msg)
 143                 return;
 144
 145         near_info("Send SNEP / Hs frame");
 146
 147         near_snep_core_response_with_info(client_fd, NEAR_SNEP_RESP_SUCCESS,
 148                                                                 msg->data, msg->length);
 149
 150         g_free(msg->data);
 151         g_free(msg);
 152 }
 153
 154 /*
 155  * This code will read the ndef message.
 156  * return        <0 on error
 157  *              ==0 if not more bytes
 158  *              >0 if there's still some data to read
 159  */
 160 static int snep_core_read_ndef(int client_fd,
 161                                         struct p2p_snep_data *snep_data)
 162 {
 163         int bytes_recv, remaining_bytes;
 164
 165         DBG("");
 166
 167         remaining_bytes = snep_data->nfc_data_length -
 168                                         snep_data->nfc_data_current_length;
 169
 170         bytes_recv = recv(client_fd, snep_data->nfc_data_ptr, remaining_bytes,
 171                                                                 MSG_DONTWAIT);
 172         if (bytes_recv < 0) {
 173                 near_error("%d %s", bytes_recv, strerror(errno));
 174
 175                 /* Some more data should show up */
 176                 if (errno == EAGAIN)
 177                         return EAGAIN;  /* Positive !!*/
 178
 179                 goto out;
 180         }
 181
 182         snep_data->nfc_data_current_length += bytes_recv;
 183         snep_data->nfc_data_ptr += bytes_recv;
 184
 185         /* Is the read complete ? */
 186         if (snep_data->nfc_data_length == snep_data->nfc_data_current_length)
 187                 return 0;
 188
 189         if (!snep_data->respond_continue) {
 190                 snep_data->respond_continue = TRUE;
 191                 near_snep_core_response_noinfo(client_fd, NEAR_SNEP_RESP_CONTINUE);
 192         }
 193
 194         return 1;
 195
 196 out:
 197         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 198
 199         return -errno;          /* Negative on error */
 200 }
 201
 202 static void free_snep_core_fragment(gpointer data)
 203 {
 204         struct snep_fragment *fragment = data;
 205
 206         if (fragment)
 207                 g_free(fragment->data);
 208
 209         g_free(fragment);
 210         fragment = NULL;
 211 }
 212
 213 static void free_snep_core_push_data(gpointer userdata, int status)
 214 {
 215         struct p2p_snep_put_req_data *data;
 216
 217         DBG("");
 218
 219         if (!userdata)
 220                 return;
 221
 222         data = (struct p2p_snep_put_req_data *) userdata;
 223
 224         close(data->fd);
 225
 226         if (data->cb)
 227                 data->cb(data->adapter_idx, data->target_idx, status);
 228
 229         if (data->watch > 0)
 230                 g_source_remove(data->watch);
 231
 232         g_slist_free_full(data->fragments, free_snep_core_fragment);
 233         g_free(data);
 234 }
 235
 236 static int snep_core_send_fragment(struct p2p_snep_put_req_data *req)
 237 {
 238         struct snep_fragment *fragment;
 239         int err;
 240
 241         DBG("");
 242
 243         if (!req || !req->fragments ||
 244                 g_slist_length(req->fragments) == 0)
 245                 return -EINVAL;
 246
 247         fragment = req->fragments->data;
 248
 249         err = send(req->fd, fragment->data, fragment->len, 0);
 250
 251         req->fragments = g_slist_remove(req->fragments, fragment);
 252         g_free(fragment->data);
 253         g_free(fragment);
 254
 255         return err;
 256 }
 257
 258 static int snep_core_push_response(struct p2p_snep_put_req_data *req)
 259 {
 260         struct p2p_snep_resp_frame frame;
 261         uint8_t *ndef;
 262         uint32_t ndef_len;
 263         int bytes_recv, err;
 264
 265         DBG("");
 266
 267         bytes_recv = recv(req->fd, &frame, sizeof(frame), 0);
 268         if (bytes_recv < 0) {
 269                 near_error("Read SNEP frame error %d %s", bytes_recv,
 270                                                         strerror(errno));
 271                 return bytes_recv;
 272         }
 273
 274         /* Check frame length */
 275         frame.length = g_ntohl(frame.length);
 276
 277         DBG("Response 0x%x %p", frame.response, &frame);
 278         switch (frame.response) {
 279         case NEAR_SNEP_RESP_CONTINUE:
 280                 while (g_slist_length(req->fragments) != 0) {
 281                         err = snep_core_send_fragment(req);
 282                         if (err < 0)
 283                                 return err;
 284                 }
 285
 286                 return frame.response;
 287
 288         case NEAR_SNEP_RESP_SUCCESS:
 289                 if (frame.length == 0)
 290                         return 0;
 291
 292                 /* Get the incoming data */
 293                 ndef_len = frame.length;
 294                 ndef = g_try_malloc0(ndef_len);
 295                 if (!ndef)
 296                         return -ENOMEM;
 297
 298                 bytes_recv = recv(req->fd, ndef, ndef_len, 0);
 299                 if (bytes_recv < 0) {
 300                         near_error("Read SNEP frame error: %d %s", bytes_recv,
 301                                                         strerror(errno));
 302                         return bytes_recv;
 303                 }
 304
 305                 /* Not enough bytes */
 306                 if (bytes_recv < 6)
 307                         return -EINVAL;
 308
 309                 if (strncmp((char *)(ndef + 3), "Hs", 2) == 0)
 310                         near_snep_core_parse_handover_record(req->fd, ndef,
 311                                                                 ndef_len);
 312
 313                 g_free(ndef);
 314
 315                 return 0;
 316         }
 317
 318         return -1;
 319 }
 320
 321 static gboolean snep_core_push_event(GIOChannel *channel,
 322                                 GIOCondition condition, gpointer data)
 323 {
 324         int err;
 325
 326         DBG("push_event condition 0x%x", condition);
 327
 328         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
 329
 330                 near_error("Error with SNEP channel");
 331
 332                 free_snep_core_push_data(data, -1);
 333
 334                 return FALSE;
 335         }
 336
 337         err = snep_core_push_response(data);
 338         if (err <= 0) {
 339                 free_snep_core_push_data(data, err);
 340
 341                 return FALSE;
 342         }
 343
 344         return TRUE;
 345 }
 346
 347 static int snep_core_push_prepare_fragments(struct p2p_snep_put_req_data *req,
 348                                                 struct near_ndef_message *ndef)
 349 {
 350         struct snep_fragment *fragment;
 351         uint32_t max_fragment_len;
 352
 353         DBG("");
 354
 355         max_fragment_len = NEAR_SNEP_REQ_MAX_FRAGMENT_LENGTH;
 356
 357         while (ndef->offset < ndef->length) {
 358
 359                 fragment = g_try_malloc0(sizeof(struct snep_fragment));
 360                 if (!fragment)
 361                         return -ENOMEM;
 362
 363                 if (max_fragment_len <= (ndef->length - ndef->offset))
 364                         fragment->len = max_fragment_len;
 365                 else
 366                         fragment->len = ndef->length - ndef->offset;
 367
 368                 fragment->data = g_try_malloc0(fragment->len);
 369                 if (!fragment->data) {
 370                         g_free(fragment);
 371                         return -ENOMEM;
 372                 }
 373
 374                 memcpy(fragment->data, ndef->data + ndef->offset,
 375                                         fragment->len);
 376                 ndef->offset += fragment->len;
 377                 req->fragments = g_slist_append(req->fragments, fragment);
 378         }
 379
 380         return 0;
 381 }
 382
 383 static bool snep_core_process_request(int client_fd,
 384                                         struct p2p_snep_data *snep_data,
 385                                         near_server_io req_get,
 386                                         near_server_io req_put)
 387 {
 388         bool ret;
 389         int err;
 390
 391         DBG("request %d", snep_data->request);
 392
 393         /* Now, we process the request code */
 394         switch (snep_data->request) {
 395         case NEAR_SNEP_REQ_PUT:
 396                 DBG("NEAR_SNEP_REQ_PUT");
 397                 if (req_put)
 398                         ret = (*req_put)(client_fd, snep_data);
 399                 else {
 400                         near_snep_core_response_noinfo(client_fd,
 401                                                 NEAR_SNEP_RESP_NOT_IMPL);
 402                         ret = true;
 403                 }
 404
 405                 /* free and leave */
 406                 g_hash_table_remove(snep_client_hash,
 407                                                 GINT_TO_POINTER(client_fd));
 408                 break;
 409
 410         case NEAR_SNEP_REQ_GET:
 411                 DBG("NEAR_SNEP_REQ_GET");
 412                 if (req_get)
 413                         ret =  (*req_get)(client_fd, snep_data);
 414                 else {
 415                         near_snep_core_response_noinfo(client_fd,
 416                                                 NEAR_SNEP_RESP_NOT_IMPL);
 417                         ret = true;
 418                 }
 419
 420                 /* If there's some fragments, don't delete before the CONT */
 421                 if (!snep_data->req) {
 422                         /* free and leave */
 423                         DBG("Clean Table");
 424                         g_hash_table_remove(snep_client_hash,
 425                                                 GINT_TO_POINTER(client_fd));
 426                 }
 427                 break;
 428
 429         case NEAR_SNEP_REQ_REJECT:
 430                 DBG("NEAR_SNEP_REQ_REJECT");
 431                 if (!snep_data->req->fragments) {
 432                         near_error("error: NEAR_SNEP_REQ_REJECT but no fragment");
 433                         ret = false;
 434                 }
 435                 else {
 436                         ret = true;
 437                 }
 438
 439                 g_slist_free_full(snep_data->req->fragments,
 440                                                 free_snep_core_fragment);
 441                 g_slist_free(snep_data->req->fragments);
 442
 443                 g_hash_table_remove(snep_client_hash,
 444                                                 GINT_TO_POINTER(client_fd));
 445
 446                 break;
 447
 448         case NEAR_SNEP_REQ_CONTINUE:
 449                 /*
 450                  * NEAR_SNEP_REQ_CONTINUE indicates that we have to send the
 451                  * remaining fragments...
 452                  */
 453
 454                 if (!snep_data->req) {
 455                         ret = true;
 456                         break;
 457                 }
 458
 459                 DBG("NEAR_SNEP_REQ_CONTINUE");
 460                 if (!snep_data->req->fragments) {
 461                         near_error("error: NEAR_SNEP_REQ_CONTINUE but no fragment");
 462                         ret = false;
 463                         goto leave_cont;
 464                 }
 465
 466                 /* Send fragments, one after the other (no ack expected) */
 467                 while (g_slist_length(snep_data->req->fragments) != 0) {
 468                         err = snep_core_send_fragment(snep_data->req);
 469                         if (err < 0) {
 470                                 ret = false;
 471                                 goto leave_cont;
 472                         }
 473                 }
 474
 475                 ret = true;
 476
 477 leave_cont:
 478                 /* No more fragment to send, clean memory */
 479                 g_slist_free_full(snep_data->req->fragments,
 480                                                 free_snep_core_fragment);
 481                 g_slist_free(snep_data->req->fragments);
 482
 483                 g_hash_table_remove(snep_client_hash,
 484                                                 GINT_TO_POINTER(client_fd));
 485
 486                 break;
 487
 488         default:
 489                 near_error("Unsupported SNEP request code");
 490                 ret = false;
 491                 break;
 492         }
 493
 494         return ret;
 495 }
 496
 497 /*
 498  * SNEP Core: read function
 499  *      This function handles SNEP REQUEST codes:
 500  *      GET, PUT and CONTINUE (REJECT is not handled).
 501  *
 502  *      We read the first 6 bytes (the header) and check
 503  *      - the read size ( should be 6 )
 504  *      - the version (on MAJOR)
 505  *
 506  *      Then, we check snep_data. If it exists, it means that we are in
 507  *      a fragment/continue situation (a 1st fragment was sent, and we
 508  *      expect a CONTINUE for the remaining bytes).
 509  *      If there's no existing snep_data, we create a new one and read the
 510  *      missing bytes (llcp removes fragmentation issues)
 511  *
 512  */
 513 bool near_snep_core_read(int client_fd,
 514                                 uint32_t adapter_idx, uint32_t target_idx,
 515                                 near_tag_io_cb cb,
 516                                 near_server_io req_get,
 517                                 near_server_io req_put,
 518                                 gpointer data)
 519 {
 520         struct p2p_snep_data *snep_data;
 521         struct p2p_snep_req_frame frame;
 522         int bytes_recv, ret;
 523         uint32_t ndef_length;
 524
 525         DBG("");
 526
 527         /* Check previous/pending snep_data */
 528         snep_data = g_hash_table_lookup(snep_client_hash,
 529                                         GINT_TO_POINTER(client_fd));
 530
 531         /*
 532          * If snep data is already there, and there are more bytes to read
 533          * we just go ahead and read more fragments from the client.
 534          */
 535         if (snep_data &&
 536                         snep_data->nfc_data_length !=
 537                                         snep_data->nfc_data_current_length) {
 538                 ret = snep_core_read_ndef(client_fd, snep_data);
 539                 if (ret)
 540                         return ret;
 541
 542                 goto process_request;
 543         }
 544
 545         /*
 546          * We already got something from this client, we should try
 547          * to continue reading.
 548          */
 549         /* TODO Try with PEEK */
 550         bytes_recv = recv(client_fd, &frame, sizeof(frame), 0);
 551         if (bytes_recv < 0) {
 552                 near_error("Read error SNEP %d %s", bytes_recv,
 553                                                         strerror(errno));
 554                 return false;
 555         }
 556
 557         /* Check frame size */
 558         if (bytes_recv != sizeof(frame)) {
 559                 near_error("Bad frame size: %d", bytes_recv);
 560                 return false;
 561         }
 562
 563         /* If major is different, send UNSUPPORTED VERSION */
 564         if (NEAR_SNEP_MAJOR(frame.version) != NEAR_SNEP_MAJOR(NEAR_SNEP_VERSION)) {
 565                 near_error("Unsupported version (%d)", frame.version);
 566                 near_snep_core_response_noinfo(client_fd, NEAR_SNEP_RESP_VERSION);
 567                 return true;
 568         }
 569
 570         /*
 571          * This is a fragmentation SNEP operation since we have pending
 572          * frames. But the ndef length and the current data length are
 573          * identical. So this is a CONTINUE for a fragmented GET, and
 574          * we should just process a CONTINUE frame and send the fragments
 575          * back to the client. This will be done from snep_core_process_request().
 576          */
 577         if (snep_data) {
 578                 snep_data->request = frame.request;
 579                 goto process_request;
 580         }
 581
 582         /* This is a new request from the client */
 583         snep_data = g_try_malloc0(sizeof(struct p2p_snep_data));
 584         if (!snep_data)
 585                 return false;
 586
 587         /* the whole frame length */
 588         ndef_length = GINT_FROM_BE(frame.length);
 589
 590         snep_data->nfc_data = g_try_malloc0(ndef_length + TLV_SIZE);
 591         if (!snep_data->nfc_data) {
 592                 g_free(snep_data);
 593                 return false;
 594         }
 595
 596         /* fill the struct */
 597         snep_data->nfc_data_length = ndef_length;
 598         snep_data->nfc_data_ptr = snep_data->nfc_data;
 599         snep_data->adapter_idx = adapter_idx;
 600         snep_data->target_idx = target_idx;
 601         snep_data->request = frame.request;
 602         snep_data->respond_continue = FALSE;
 603         snep_data->cb = cb;
 604
 605         /* Add to the client hash table */
 606         g_hash_table_insert(snep_client_hash,
 607                                         GINT_TO_POINTER(client_fd), snep_data);
 608
 609         if (ndef_length > 0) {
 610                 if ((frame.request == NEAR_SNEP_REQ_GET) ||
 611                                 (frame.request == NEAR_SNEP_REQ_PUT)) {
 612                         /* We should read the missing bytes */
 613                         ret = snep_core_read_ndef(client_fd, snep_data);
 614                         if (ret)
 615                                 return ret;
 616                 }
 617         }
 618
 619 process_request:
 620         return snep_core_process_request(client_fd, snep_data,
 621                                                         req_get, req_put);
 622
 623 }
 624
 625 /*
 626  * send a response frame with some datas. If the frame is too long, we
 627  * have to fragment the frame, using snep fragmentation protocol.
 628  * Return:
 629  * < 0 if error
 630  * 0 if no fragment;
 631  * > 0 if there's still some fragments
 632  *
 633  */
 634 static int near_snep_core_response(int fd, struct p2p_snep_put_req_data *req,
 635                 uint8_t resp_code, struct near_ndef_message *ndef)
 636 {
 637         struct p2p_snep_req_frame header;
 638         struct snep_fragment *fragment;
 639         uint32_t max_fragment_len;
 640         bool fragmenting;
 641         int err;
 642         int snep_req_header_length, snep_additional_length;
 643
 644         DBG("resp: 0x%02X", resp_code);
 645
 646         max_fragment_len = NEAR_SNEP_REQ_MAX_FRAGMENT_LENGTH;
 647         header.version = NEAR_SNEP_VERSION;
 648
 649         if (resp_code == NEAR_SNEP_REQ_GET) {   /* Get for android */
 650                 snep_req_header_length = NEAR_SNEP_REQ_GET_HEADER_LENGTH;
 651                 snep_additional_length = 4;     /* 4 Acceptable Length */
 652         } else {
 653                 snep_req_header_length = NEAR_SNEP_REQ_PUT_HEADER_LENGTH;
 654                 snep_additional_length = 0;
 655         }
 656
 657         header.length = GUINT32_TO_BE(ndef->length + snep_additional_length);
 658         header.request = resp_code;
 659
 660         fragment = g_try_malloc0(sizeof(struct snep_fragment));
 661
 662         if (!fragment) {
 663                 err = -ENOMEM;
 664                 goto error;
 665         }
 666
 667         if (max_fragment_len >= (ndef->length + snep_req_header_length)) {
 668                 fragment->len = ndef->length + snep_req_header_length;
 669                 fragmenting = false;
 670         } else {
 671                 fragment->len = max_fragment_len;
 672                 fragmenting = true;
 673         }
 674
 675         fragment->data = g_try_malloc0(fragment->len);
 676         if (!fragment->data) {
 677                 g_free(fragment);
 678                 err = ENOMEM;
 679                 goto error;
 680         }
 681
 682         /* Header to data - common header */
 683         memcpy(fragment->data, (uint8_t *)&header, NEAR_SNEP_REQ_PUT_HEADER_LENGTH);
 684
 685         /* if GET, we add the Acceptable length */
 686         if (header.request == NEAR_SNEP_REQ_GET)
 687                 near_put_be32(snep_req_header_length,
 688                                 fragment->data + NEAR_SNEP_REQ_PUT_HEADER_LENGTH);
 689
 690         if (fragmenting) {
 691                 memcpy(fragment->data + snep_req_header_length, ndef->data,
 692                                 max_fragment_len - snep_req_header_length);
 693                 ndef->offset = max_fragment_len - snep_req_header_length;
 694
 695                 err = snep_core_push_prepare_fragments(req, ndef);
 696                 if (err < 0) {
 697                         g_free(fragment->data);
 698                         g_free(fragment);
 699                         goto error;
 700                 }
 701         } else {
 702                 memcpy(fragment->data + snep_req_header_length,
 703                                                 ndef->data, ndef->length);
 704         }
 705
 706         err = send(fd, fragment->data, fragment->len, 0);
 707         if (err < 0) {
 708                 near_error("Sending failed %d", err);
 709                 g_free(fragment->data);
 710                 g_free(fragment);
 711                 goto error;
 712         }
 713
 714         g_free(fragment->data);
 715         g_free(fragment);
 716
 717         return 0;
 718
 719 error:
 720         if (req)
 721                 free_snep_core_push_data(req, err);
 722
 723         return err;
 724 }
 725
 726 void near_snep_core_response_with_info(int client_fd, uint8_t response,
 727                                 uint8_t *data, int length)
 728 {
 729         struct p2p_snep_data *snep_data;
 730         struct p2p_snep_put_req_data *req;
 731         struct near_ndef_message *ndef;
 732
 733         DBG("Response with info 0x%x (len:%d)", response, length);
 734
 735         req = NULL;
 736         ndef = NULL;
 737
 738         /* get the snep data */
 739         snep_data = g_hash_table_lookup(snep_client_hash,
 740                                                 GINT_TO_POINTER(client_fd));
 741         if (!snep_data) {
 742                 DBG("snep_data not found");
 743                 goto done;
 744         }
 745
 746         /* Prepare the ndef struct */
 747         ndef = g_try_malloc0(sizeof(struct near_ndef_message));
 748         if (!ndef)
 749                 goto done;
 750
 751         ndef->data = g_try_malloc0(length);
 752         if (!ndef->data) {
 753                 g_free(ndef);
 754                 ndef = NULL;
 755                 goto done;
 756         }
 757
 758         /* Fill the ndef */
 759         ndef->length = length;
 760         ndef->offset = 0;
 761         memcpy(ndef->data, data, length);
 762
 763         ndef->offset = 0;
 764
 765         /* Now prepare req struct */
 766         req = g_try_malloc0(sizeof(struct p2p_snep_put_req_data));
 767         if (!req)
 768                 goto done;
 769
 770         /* Prepare the callback */
 771         snep_data->req = req;
 772
 773         req->fd = client_fd;
 774         req->adapter_idx = snep_data->adapter_idx;
 775         req->target_idx = snep_data->target_idx;
 776         req->cb = snep_data->cb;
 777
 778         /* send it !*/
 779         near_snep_core_response(client_fd, req, response, ndef);
 780
 781 done:
 782         /* If no fragment, free mem */
 783         if (req) {
 784                 if (req->fragments == 0) {
 785                         g_free(req);
 786                         snep_data->req = NULL;
 787                 }
 788         }
 789
 790         if (ndef)
 791                 g_free(ndef->data);
 792         g_free(ndef);
 793 }
 794
 795 /* SNEP Core: on P2P push */
 796 int near_snep_core_push(int fd, uint32_t adapter_idx, uint32_t target_idx,
 797                         struct near_ndef_message *ndef,
 798                         near_device_io_cb cb,
 799                         gpointer data)
 800 {
 801         struct p2p_snep_put_req_data *req;
 802         GIOChannel *channel;
 803         uint8_t resp_code;
 804         int err;
 805
 806         DBG("");
 807
 808         req = g_try_malloc0(sizeof(struct p2p_snep_put_req_data));
 809         if (!req) {
 810                 err = -ENOMEM;
 811                 goto error;
 812         }
 813
 814         channel = g_io_channel_unix_new(fd);
 815         g_io_channel_set_close_on_unref(channel, TRUE);
 816
 817         req->fd = fd;
 818         req->adapter_idx = adapter_idx;
 819         req->target_idx = target_idx;
 820         req->cb = cb;
 821         ndef->offset = 0;
 822         req->watch = g_io_add_watch(channel, G_IO_IN | G_IO_HUP | G_IO_NVAL |
 823                                         G_IO_ERR, snep_core_push_event,
 824                                         (gpointer) req);
 825
 826         /* Check if Hr or Hs for Handover over SNEP */
 827         if (*(char *)(ndef->data + 3) == 'H')
 828                 resp_code = NEAR_SNEP_REQ_GET;          /* Get for android */
 829         else
 830                 resp_code = NEAR_SNEP_REQ_PUT;
 831
 832         return near_snep_core_response(fd, req, resp_code, ndef);
 833
 834 error:
 835         free_snep_core_push_data(req, err);
 836
 837         return err;
 838
 839 }
 840
 841 /* SNEP core functions: close */
 842 void near_snep_core_close(int client_fd, int err, gpointer data)
 843 {
 844         struct p2p_snep_data *snep_data;
 845
 846         DBG("");
 847
 848         snep_data = g_hash_table_lookup(snep_client_hash,
 849                                         GINT_TO_POINTER(client_fd));
 850         if (!snep_data)
 851                 return;
 852
 853         snep_data->cb(snep_data->adapter_idx, snep_data->target_idx, err);
 854
 855         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 856 }
 857
 858 int __near_snep_core_init(void)
 859 {
 860         snep_client_hash = g_hash_table_new_full(g_direct_hash,
 861                                                         g_direct_equal, NULL,
 862                                                         free_snep_core_client);
 863
 864         return 0;
 865 }
 866
 867 void __near_snep_core_cleanup(void)
 868 {
 869         g_hash_table_destroy(snep_client_hash);
 870         snep_client_hash = NULL;
 871 }