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 /* TODO: Right now it is dummy, need to get correct value
  64  * from lower layers */
  65 #define SNEP_REQ_MAX_FRAGMENT_LENGTH 128
  66
  67 struct p2p_snep_data {
  68         uint8_t *nfc_data;
  69         uint32_t nfc_data_length;
  70         uint32_t nfc_data_current_length;
  71         uint8_t *nfc_data_ptr;
  72         uint32_t adapter_idx;
  73         uint32_t target_idx;
  74         gboolean respond_continue;
  75         near_tag_io_cb cb;
  76 };
  77
  78 struct snep_fragment {
  79         uint32_t len;
  80         uint8_t *data;
  81 };
  82
  83 struct p2p_snep_put_req_data {
  84         uint8_t fd;
  85         uint32_t adapter_idx;
  86         uint32_t target_idx;
  87         near_device_io_cb cb;
  88         guint watch;
  89
  90         GSList *fragments;
  91 };
  92
  93 struct p2p_snep_req_frame {
  94         uint8_t version;
  95         uint8_t request;
  96         uint32_t length;
  97         uint8_t ndef[];
  98 } __attribute__((packed));
  99
 100 struct p2p_snep_resp_frame {
 101         uint8_t version;
 102         uint8_t response;
 103         uint32_t length;
 104         uint8_t info[];
 105 } __attribute__((packed));
 106
 107 static GHashTable *snep_client_hash = NULL;
 108
 109 static void free_snep_client(gpointer data)
 110 {
 111         struct p2p_snep_data *snep_data = data;
 112
 113         g_free(snep_data->nfc_data);
 114         g_free(snep_data);
 115 }
 116
 117 static void snep_response_noinfo(int client_fd, uint8_t response)
 118 {
 119         struct p2p_snep_resp_frame resp;
 120
 121         DBG("Response 0x%x", response);
 122
 123         resp.version = SNEP_VERSION;
 124         resp.response = response;
 125         resp.length = 0;
 126
 127         send(client_fd, &resp, sizeof(resp), 0);
 128 }
 129
 130 static void snep_close(int client_fd, int err)
 131 {
 132         struct p2p_snep_data *snep_data;
 133
 134         DBG("");
 135
 136         snep_data = g_hash_table_lookup(snep_client_hash,
 137                                         GINT_TO_POINTER(client_fd));
 138         if (snep_data == NULL)
 139                 return;
 140
 141         snep_data->cb(snep_data->adapter_idx, snep_data->target_idx, err);
 142
 143         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 144 }
 145
 146 static near_bool_t snep_read_ndef(int client_fd,
 147                                         struct p2p_snep_data *snep_data)
 148 {
 149         int bytes_recv, remaining_bytes;
 150         struct near_device *device;
 151         GList *records;
 152
 153         DBG("");
 154
 155         remaining_bytes = snep_data->nfc_data_length -
 156                                 snep_data->nfc_data_current_length;
 157
 158         DBG("Remaining bytes %d", remaining_bytes);
 159
 160         bytes_recv = recv(client_fd, snep_data->nfc_data_ptr, remaining_bytes,
 161                                                                 MSG_DONTWAIT);
 162         if (bytes_recv < 0) {
 163                 near_error("%d %s", bytes_recv, strerror(errno));
 164
 165                 /* Some more data should show up */
 166                 if (errno == EAGAIN)
 167                         return TRUE;
 168
 169                 goto out;
 170         }
 171
 172         DBG("Received %d bytes", bytes_recv);
 173
 174         snep_data->nfc_data_current_length += bytes_recv;
 175         snep_data->nfc_data_ptr += bytes_recv;
 176
 177         if (snep_data->nfc_data_length != snep_data->nfc_data_current_length) {
 178                 if (snep_data->respond_continue == FALSE) {
 179                         DBG("Continue");
 180                         snep_data->respond_continue = TRUE;
 181                         snep_response_noinfo(client_fd, SNEP_RESP_CONTINUE);
 182                 }
 183
 184                 return TRUE;
 185         }
 186
 187         snep_response_noinfo(client_fd, SNEP_RESP_SUCCESS);
 188         if (near_device_add_data(snep_data->adapter_idx, snep_data->target_idx,
 189                                         snep_data->nfc_data,
 190                                         snep_data->nfc_data_length) < 0)
 191                 goto out;
 192
 193         device = near_device_get_device(snep_data->adapter_idx,
 194                                                 snep_data->target_idx);
 195         if (device == NULL)
 196                 goto out;
 197
 198         records = near_ndef_parse(snep_data->nfc_data,
 199                                 snep_data->nfc_data_length);
 200         near_device_add_records(device, records, snep_data->cb, 0);
 201
 202 out:
 203         g_hash_table_remove(snep_client_hash, GINT_TO_POINTER(client_fd));
 204
 205         return FALSE;
 206 }
 207
 208 static near_bool_t snep_read(int client_fd,
 209                                 uint32_t adapter_idx, uint32_t target_idx,
 210                                 near_tag_io_cb cb)
 211 {
 212         struct p2p_snep_data *snep_data;
 213         struct p2p_snep_req_frame frame;
 214         int bytes_recv;
 215         uint32_t ndef_length;
 216
 217         DBG("");
 218
 219         snep_data = g_hash_table_lookup(snep_client_hash,
 220                                         GINT_TO_POINTER(client_fd));
 221
 222         /*
 223          * We already got something from this client, we should try
 224          * to continue reading.
 225          */
 226         if (snep_data != NULL)
 227                 return snep_read_ndef(client_fd, snep_data);
 228
 229         /* TODO Try with PEEK */
 230         bytes_recv = recv(client_fd, &frame, sizeof(frame), 0);
 231         if (bytes_recv < 0) {
 232                 near_error("Could not read SNEP frame %d", bytes_recv);
 233                 return bytes_recv;
 234         }
 235
 236         ndef_length = GINT_FROM_BE(frame.length);
 237
 238         DBG("Allocating SNEP data %d", ndef_length);
 239
 240         snep_data = g_try_malloc0(sizeof(struct p2p_snep_data));
 241         if (snep_data == NULL)
 242                 return FALSE;
 243
 244         snep_data->nfc_data = g_try_malloc0(ndef_length + TLV_SIZE);
 245         if (snep_data->nfc_data == NULL) {
 246                 g_free(snep_data);
 247                 return FALSE;
 248         }
 249
 250         snep_data->nfc_data_length = ndef_length;
 251         snep_data->nfc_data_ptr = snep_data->nfc_data;
 252         snep_data->adapter_idx = adapter_idx;
 253         snep_data->target_idx = target_idx;
 254         snep_data->respond_continue = FALSE;
 255         snep_data->cb = cb;
 256
 257         g_hash_table_insert(snep_client_hash,
 258                                         GINT_TO_POINTER(client_fd), snep_data);
 259
 260         DBG("Request 0x%x", frame.request);
 261
 262         switch (frame.request) {
 263         case SNEP_REQ_CONTINUE:
 264         case SNEP_REQ_GET:
 265                 near_error("Unsupported SNEP request code");
 266                 snep_response_noinfo(client_fd, SNEP_RESP_NOT_IMPL);
 267                 return FALSE;
 268         case SNEP_REQ_PUT:
 269                 return snep_read_ndef(client_fd, snep_data);
 270         }
 271
 272         return FALSE;
 273 }
 274
 275 static void free_snep_fragment(gpointer data)
 276 {
 277         struct snep_fragment *fragment = data;
 278
 279         if (fragment != NULL)
 280                 g_free(fragment->data);
 281
 282         g_free(fragment);
 283         fragment = NULL;
 284 }
 285
 286 static void free_snep_push_data(gpointer userdata, int status)
 287 {
 288         struct p2p_snep_put_req_data *data;
 289
 290         DBG("");
 291
 292         data = (struct p2p_snep_put_req_data *)userdata;
 293
 294         close(data->fd);
 295
 296         if (data->cb)
 297                 data->cb(data->adapter_idx, data->target_idx, status);
 298
 299         if (data->watch > 0)
 300                 g_source_remove(data->watch);
 301
 302         g_slist_free_full(data->fragments, free_snep_fragment);
 303         g_free(data);
 304 }
 305
 306 static int snep_send_fragment(struct p2p_snep_put_req_data *req)
 307 {
 308         struct snep_fragment *fragment;
 309         int err;
 310
 311         DBG("");
 312
 313         if (req == NULL || req->fragments == NULL ||
 314                 g_slist_length(req->fragments) == 0)
 315                 return -EINVAL;
 316
 317         fragment = req->fragments->data;
 318
 319         err = send(req->fd, fragment->data, fragment->len, 0);
 320
 321         req->fragments = g_slist_remove(req->fragments, fragment);
 322         g_free(fragment->data);
 323         g_free(fragment);
 324
 325         return err;
 326 }
 327
 328 static int snep_push_response(struct p2p_snep_put_req_data *req)
 329 {
 330         struct p2p_snep_resp_frame frame;
 331         int bytes_recv, err;
 332
 333         DBG("");
 334
 335         bytes_recv = recv(req->fd, &frame, sizeof(frame), 0);
 336         if (bytes_recv < 0) {
 337                 near_error("Could not read SNEP frame %d", bytes_recv);
 338                 return bytes_recv;
 339         }
 340
 341         DBG("Response 0x%x", frame.response);
 342
 343         switch (frame.response) {
 344         case SNEP_RESP_CONTINUE:
 345                 while (g_slist_length(req->fragments) != 0) {
 346                         err = snep_send_fragment(req);
 347                         if (err < 0)
 348                                 return err;
 349                 }
 350
 351                 return frame.response;
 352
 353         case SNEP_RESP_SUCCESS:
 354                 return 0;
 355         }
 356
 357         return -1;
 358 }
 359
 360 static gboolean snep_push_event(GIOChannel *channel,
 361                                 GIOCondition condition, gpointer data)
 362 {
 363         int err;
 364
 365         DBG("condition 0x%x", condition);
 366
 367         if (condition & (G_IO_NVAL | G_IO_ERR | G_IO_HUP)) {
 368
 369                 near_error("Error with SNEP channel");
 370
 371                 free_snep_push_data(data, -1);
 372
 373                 return FALSE;
 374         }
 375
 376         err = snep_push_response(data);
 377         if (err <= 0) {
 378                 free_snep_push_data(data, err);
 379
 380                 return FALSE;
 381         }
 382
 383         return TRUE;
 384 }
 385
 386 static int snep_push_prepare_fragments(struct p2p_snep_put_req_data *req,
 387                                                 struct near_ndef_message *ndef)
 388 {
 389         struct snep_fragment *fragment;
 390         uint32_t max_fragment_len;
 391
 392         DBG("");
 393
 394         max_fragment_len = SNEP_REQ_MAX_FRAGMENT_LENGTH;
 395
 396         while (ndef->offset < ndef->length) {
 397
 398                 fragment = g_try_malloc0(sizeof(struct snep_fragment));
 399                 if (fragment == NULL)
 400                         return -ENOMEM;
 401
 402                 if (max_fragment_len <= (ndef->length - ndef->offset))
 403                         fragment->len = max_fragment_len;
 404                 else
 405                         fragment->len = ndef->length - ndef->offset;
 406
 407                 fragment->data = g_try_malloc0(fragment->len);
 408                 if (fragment->data == NULL) {
 409                         g_free(fragment);
 410                         return -ENOMEM;
 411                 }
 412
 413                 memcpy(fragment->data, ndef->data + ndef->offset,
 414                                         fragment->len);
 415                 ndef->offset += fragment->len;
 416                 req->fragments = g_slist_append(req->fragments, fragment);
 417         }
 418
 419         return 0;
 420 }
 421
 422 static int snep_push(int fd, uint32_t adapter_idx, uint32_t target_idx,
 423                         struct near_ndef_message *ndef,
 424                         near_device_io_cb cb)
 425 {
 426         struct p2p_snep_put_req_data *req;
 427         struct p2p_snep_req_frame header;
 428         struct snep_fragment *fragment;
 429         uint32_t max_fragment_len;
 430         GIOChannel *channel;
 431         gboolean fragmenting;
 432         int err;
 433
 434         DBG("");
 435
 436         req = g_try_malloc0(sizeof(struct p2p_snep_put_req_data));
 437         if (req == NULL) {
 438                 err = -ENOMEM;
 439                 goto error;
 440         }
 441
 442         channel = g_io_channel_unix_new(fd);
 443         g_io_channel_set_close_on_unref(channel, TRUE);
 444
 445         req->fd = fd;
 446         req->adapter_idx = adapter_idx;
 447         req->target_idx = target_idx;
 448         req->cb = cb;
 449         ndef->offset = 0;
 450         req->watch = g_io_add_watch(channel, G_IO_IN | G_IO_HUP | G_IO_NVAL |
 451                                         G_IO_ERR, snep_push_event,
 452                                         (gpointer) req);
 453
 454         max_fragment_len = SNEP_REQ_MAX_FRAGMENT_LENGTH;
 455         header.version = SNEP_VERSION;
 456         header.request = SNEP_REQ_PUT;
 457         header.length = GUINT32_TO_BE(ndef->length);
 458
 459         fragment = g_try_malloc0(sizeof(struct snep_fragment));
 460         if (fragment == NULL) {
 461                 err = -ENOMEM;
 462                 goto error;
 463         }
 464
 465         if (max_fragment_len >= (ndef->length + SNEP_REQ_PUT_HEADER_LENGTH)) {
 466                 fragment->len = ndef->length + SNEP_REQ_PUT_HEADER_LENGTH;
 467                 fragmenting = FALSE;
 468         } else {
 469                 fragment->len = max_fragment_len;
 470                 fragmenting = TRUE;
 471         }
 472
 473         fragment->data = g_try_malloc0(fragment->len);
 474         if (fragment->data == NULL) {
 475                 g_free(fragment);
 476                 err = ENOMEM;
 477                 goto error;
 478         }
 479
 480         memcpy(fragment->data, (uint8_t *)&header,
 481                                 SNEP_REQ_PUT_HEADER_LENGTH);
 482
 483         if (fragmenting == TRUE) {
 484                 memcpy(fragment->data + SNEP_REQ_PUT_HEADER_LENGTH,
 485                         ndef->data,
 486                         max_fragment_len - SNEP_REQ_PUT_HEADER_LENGTH);
 487                 ndef->offset = max_fragment_len - SNEP_REQ_PUT_HEADER_LENGTH;
 488
 489                 err = snep_push_prepare_fragments(req, ndef);
 490                 if (err < 0) {
 491                         g_free(fragment->data);
 492                         g_free(fragment);
 493                         goto error;
 494                 }
 495
 496         } else {
 497                 memcpy(fragment->data + SNEP_REQ_PUT_HEADER_LENGTH,
 498                                         ndef->data, ndef->length);
 499         }
 500
 501         err = send(fd, fragment->data, fragment->len, 0);
 502         if (err < 0) {
 503                 near_error("Sending failed %d", err);
 504                 g_free(fragment->data);
 505                 g_free(fragment);
 506
 507                 goto error;
 508         }
 509
 510         g_free(fragment->data);
 511         g_free(fragment);
 512
 513         return 0;
 514
 515 error:
 516         free_snep_push_data(req, err);
 517
 518         return err;
 519 }
 520
 521 struct near_p2p_driver snep_driver = {
 522         .name = "SNEP",
 523         .service_name = NEAR_DEVICE_SN_SNEP,
 524         .read = snep_read,
 525         .push = snep_push,
 526         .close = snep_close,
 527 };
 528
 529 int snep_init(void)
 530 {
 531         snep_client_hash = g_hash_table_new_full(g_direct_hash, g_direct_equal,
 532                                                         NULL, free_snep_client);
 533
 534         return near_p2p_register(&snep_driver);
 535 }
 536
 537 void snep_exit(void)
 538 {
 539         near_p2p_unregister(&snep_driver);
 540
 541         g_hash_table_destroy(snep_client_hash);
 542         snep_client_hash = NULL;
 543 }