handover: Support for handover selector mode

This code parses received handover requester records, create an Hs one
and send it back to the requester.

diff --git a/plugins/handover.c b/plugins/handover.c
index d96796d..9a5a6a9 100644 (file)
--- a/plugins/handover.c
+++ b/plugins/handover.c
@@ -31,38 +31,354 @@
 #include <linux/socket.h>
 #include <linux/nfc.h>
 
-#include <near/log.h>
 #include <near/types.h>
+#include <near/log.h>
 #include <near/adapter.h>
 #include <near/device.h>
+#include <near/tag.h>
 #include <near/ndef.h>
 #include <near/tlv.h>
 
 #include "p2p.h"
 
-static near_bool_t handover_read(int client_fd,
+#define NDEF_HR_MSG_MIN_LENGTH 0x06
+#define HR_HEADER_SIZE 6               /* header (1) + type len (1) +
+                                       *  payload len (1) + rec type (2) 'Hx'
+                                       *  + version (1)
+                                       */
+
+#define RECORD_TYPE_WKT_ALTERNATIVE_CARRIER 0x0a
+
+enum loop_stage_flag {
+       STATE_MAIN_NDEF         = 0x00,
+       STATE_CFG_RECORD        = 0x01,
+};
+
+static GHashTable *hr_ndef_hash = NULL;
+
+struct extra_ndef {
+       uint8_t *ndef;
+       uint8_t length;
+};
+
+struct hr_ndef {
+       uint8_t *ndef;
+       uint16_t cur_ptr;
+       int cur_record_len;
+       int missing_bytes;
+       uint32_t adapter_idx;
+       uint32_t target_idx;
+       near_tag_io_cb cb;
+       int extra_ndef_count;
+       int block_free_size;
+       near_bool_t cfg_record_state;
+       near_bool_t in_extra_read;
+};
+
+static void free_hr_ndef(gpointer data)
+{
+       struct hr_ndef *ndef = data;
+
+       if (ndef != NULL)
+               g_free(ndef->ndef);
+       g_free(ndef);
+}
+
+/* Parse an incoming handover buffer*/
+static int handover_ndef_parse(int client_fd, struct hr_ndef *ndef)
+{
+       int err;
+       GList *records;
+       struct near_ndef_message *msg;
+
+       DBG("");
+
+       if ((ndef->ndef == NULL) ||
+                       (ndef->cur_ptr < NDEF_HR_MSG_MIN_LENGTH)) {
+               err = -EINVAL;
+               goto fail;
+       }
+
+       /* call the global parse function */
+       records = near_ndef_parse(ndef->ndef, ndef->cur_ptr);
+       if (records == NULL) {
+               err = -ENOMEM;
+               goto fail;
+       }
+
+       /*
+        * The first entry on the record list is the Hr record.
+        * We build the Hs based on it.
+        */
+       msg = near_ndef_prepare_handover_record("Hs", records->data,
+                                                       NEAR_CARRIER_BLUETOOTH);
+
+       near_info("Send Hs frame");
+       err = send(client_fd, msg->data, msg->length, MSG_DONTWAIT);
+       if (err >= 0)
+               return 0;
+
+fail:
+       near_error("ndef parsing failed (%d)", err);
+
+       return err;
+}
+
+static void handover_close(int client_fd, int err)
+{
+       struct hr_ndef *ndef;
+
+       DBG("");
+
+       ndef = g_hash_table_lookup(hr_ndef_hash, GINT_TO_POINTER(client_fd));
+       if (ndef == NULL)
+               return;
+
+       g_hash_table_remove(hr_ndef_hash, GINT_TO_POINTER(client_fd));
+}
+
+static near_bool_t handover_recv_error(void)
+{
+       near_error("%s", strerror(errno));
+
+       if (errno == EAGAIN)
+               return TRUE;
+
+       return FALSE;
+}
+
+/* Add extra records right after the end of the "Hr" ndef record */
+static near_bool_t handover_read_cfg_records(int client_fd,
                                uint32_t adapter_idx, uint32_t target_idx,
                                near_tag_io_cb cb)
 {
+       struct hr_ndef *ndef;
+       int bytes_recv;
+       int ndef_size;
+
+       ndef = g_hash_table_lookup(hr_ndef_hash, GINT_TO_POINTER(client_fd));
+       if (ndef == NULL) {
+               near_error("hr_ndef should exist !!!");
+               return FALSE;
+       }
+
+       if (ndef->in_extra_read == TRUE) {
+               /* Next prepare read to complete the Hr */
+               ndef->ndef = g_try_realloc(ndef->ndef, ndef->cur_record_len +
+                               NDEF_HR_MSG_MIN_LENGTH);
+               if (ndef == NULL)
+                       return FALSE;
+
+               /* Read header bytes */
+               bytes_recv = recv(client_fd, ndef->ndef + ndef->cur_ptr,
+                               NDEF_HR_MSG_MIN_LENGTH, MSG_DONTWAIT);
+               if (bytes_recv < 0)
+                       return handover_recv_error();
+
+               /* Now, check the ndef payload size plus header bytes */
+               ndef_size = near_ndef_record_length(ndef->ndef + ndef->cur_ptr,
+                                                               bytes_recv);
+               if (ndef_size < 0)
+                       goto fail;
+
+               ndef->cur_ptr += bytes_recv;
+               ndef->missing_bytes = ndef_size - bytes_recv;
+
+               /* Next prepare read to complete the NDEF */
+               ndef->ndef = g_try_realloc(ndef->ndef, ndef->cur_record_len
+                                                               + ndef_size);
+               if (ndef->ndef == NULL) {
+                       g_free(ndef);
+                       return FALSE;
+               }
+               ndef->cur_record_len += ndef_size;
+               ndef->in_extra_read = FALSE;
+
+               return TRUE;
+       }
+
+       /* Read remaining bytes */
+       bytes_recv = recv(client_fd, ndef->ndef + ndef->cur_ptr,
+                                       ndef->missing_bytes, MSG_DONTWAIT);
+       if (bytes_recv < 0)
+               return handover_recv_error();
+
+       ndef->cur_ptr += bytes_recv;
+       ndef->missing_bytes -= bytes_recv;
+
+       /* Is the NDEF read complete ? */
+       if (ndef->missing_bytes)
+               return TRUE;    /* more bytes to come... */
+
+       ndef->extra_ndef_count--;
+       ndef->in_extra_read = TRUE;
+
+       if (ndef->extra_ndef_count == 0) {
+               /* All the bytes are read so now, parse the frame */
+               handover_ndef_parse(client_fd, ndef);
+               return FALSE;
+       }
+
+       /* Process the next NDEF */
+       return TRUE;
+
+fail:
+       near_error("Handover read NDEFs failed");
+       return FALSE;
+}
+
+static near_bool_t handover_read_hr(int client_fd,
+               uint32_t adapter_idx, uint32_t target_idx, near_tag_io_cb cb)
+{
+       int bytes_recv;
+       int extra_ndefs;
+       struct hr_ndef *ndef;
+
        DBG("");
 
-       cb(adapter_idx, target_idx, 0);
+       ndef = g_hash_table_lookup(hr_ndef_hash, GINT_TO_POINTER(client_fd));
+       if (ndef == NULL)
+               return FALSE;
+
+       /* Read remaining bytes */
+       bytes_recv = recv(client_fd, ndef->ndef + ndef->cur_ptr,
+                       ndef->missing_bytes, MSG_DONTWAIT);
+       if (bytes_recv < 0)
+               return handover_recv_error();
 
+       ndef->cur_ptr += bytes_recv;
+       ndef->missing_bytes -= bytes_recv;
+
+       /* Is the ndef "Hr" read complete or should we loop */
+       if (ndef->missing_bytes)
+               return TRUE;
+
+       /*
+        * The first NDEF frame is read. We now should determine how many
+        * extra records follow the NDEF frame.
+        * We skip the first 6 bytes (Hr header) to jump on the first record
+        */
+       extra_ndefs = near_ndef_count_records(ndef->ndef + HR_HEADER_SIZE,
+                       ndef->cur_record_len - HR_HEADER_SIZE,
+                       RECORD_TYPE_WKT_ALTERNATIVE_CARRIER);
+       if (extra_ndefs < 0)
+               goto fail;
+
+       /* There's still some extra ndefs to read */
+       ndef->extra_ndef_count = extra_ndefs;
+
+       /* End of Handover message - now process extra records */
+       ndef->in_extra_read = TRUE;
+       ndef->cfg_record_state = TRUE;
+
+       return TRUE;
+
+fail:
+       near_error("Handover read failed");
        return FALSE;
 }
 
+static near_bool_t handover_read_initialize(int client_fd,
+               uint32_t adapter_idx, uint32_t target_idx, near_tag_io_cb cb)
+{
+       int bytes_recv;
+       struct hr_ndef *ndef;
+
+       DBG("");
+
+       /* Allocate the ndef structure */
+       ndef = g_try_malloc0(sizeof(struct hr_ndef));
+       if (ndef == NULL)
+               goto fail;
+
+       /* Allocate and read frame header (6 bytes) */
+       ndef->ndef = g_try_malloc0(NDEF_HR_MSG_MIN_LENGTH);
+       if (ndef->ndef == NULL)
+               goto fail;
+
+       /* Initialize default values */
+       ndef->cur_ptr = 0;
+       ndef->cur_record_len = -1;
+       ndef->adapter_idx = adapter_idx;
+       ndef->target_idx = target_idx;
+       ndef->cb = cb;
+       ndef->cfg_record_state = FALSE;
+
+       g_hash_table_insert(hr_ndef_hash, GINT_TO_POINTER(client_fd), ndef);
+
+       /* Read header bytes (6) */
+       bytes_recv = recv(client_fd, ndef->ndef,
+                               NDEF_HR_MSG_MIN_LENGTH, MSG_DONTWAIT);
+       if (bytes_recv < 0)
+               return handover_recv_error();
+
+       /* Now, check the ndef payload size plus header bytes */
+       ndef->cur_record_len = near_ndef_record_length(ndef->ndef, bytes_recv);
+       if (ndef->cur_record_len < 0)
+               goto fail;
+
+       ndef->cur_ptr += bytes_recv;
+       ndef->missing_bytes = ndef->cur_record_len - bytes_recv;
+
+       DBG("Handover frame size is %d", ndef->cur_ptr);
+
+       /* Next prepare read to complete the read */
+       ndef->ndef = g_try_realloc(ndef->ndef, ndef->cur_record_len);
+       if (ndef->ndef == NULL)
+               goto fail;
+
+       return TRUE;
+
+fail:
+       free_hr_ndef(ndef);
+
+       return FALSE;
+}
+
+/*
+ * This function is a "dispatcher", to read Hr/Hs messages,
+ * and/or additionnal NDEF messages
+ */
+static near_bool_t handover_read(int client_fd,
+               uint32_t adapter_idx, uint32_t target_idx,
+               near_tag_io_cb cb)
+{
+       struct hr_ndef *ndef;
+
+       ndef = g_hash_table_lookup(hr_ndef_hash, GINT_TO_POINTER(client_fd));
+       if (ndef == NULL) {
+               /* First call: allocate and read header bytes*/
+               return handover_read_initialize(client_fd, adapter_idx,
+                                               target_idx, cb);
+       }
+
+       if (ndef->cfg_record_state == TRUE) {
+               return handover_read_cfg_records(client_fd, adapter_idx,
+                                                       target_idx, cb);
+       }
+
+       return handover_read_hr(client_fd, adapter_idx, target_idx, cb);
+}
+
 struct near_p2p_driver handover_driver = {
        .name = "Handover",
        .service_name = "urn:nfc:sn:handover",
        .read = handover_read,
+       .close = handover_close,
 };
 
 int handover_init(void)
 {
+       hr_ndef_hash = g_hash_table_new_full(g_direct_hash, g_direct_equal,
+                                                       NULL, free_hr_ndef);
+
        return near_p2p_register(&handover_driver);
 }
 
 void handover_exit(void)
 {
        near_p2p_unregister(&handover_driver);
+
+       g_hash_table_destroy(hr_ndef_hash);
+       hr_ndef_hash = NULL;
 }