Initial interface discovery support

Node, does not completely work yet since more SVC operations are needed.

Signed-off-by: Ayush Singh <ayushsingh1325@gmail.com>

include/greybus_protocol.h

@@ -37,11 +37,14 @@
 #define GB_SVC_TYPE_INTF_SET_PWRM_REQUEST 0x10
 #define GB_SVC_TYPE_INTF_SET_PWRM_RESPONSE                                     \
   OP_RESPONSE | GB_SVC_TYPE_INTF_SET_PWRM_REQUEST
+#define GB_SVC_TYPE_MODULE_INSERTED_REQUEST 0x1f
+#define GB_SVC_TYPE_MODULE_INSERTED_RESPONSE                                   \
+  OP_RESPONSE | GB_SVC_TYPE_MODULE_INSERTED_REQUEST
 
 #define GB_SVC_UNIPRO_HIBERNATE_MODE 0x11
 
-#define GB_SVC_SETPWRM_PWR_OK           0x00
-#define GB_SVC_SETPWRM_PWR_LOCAL        0x01
+#define GB_SVC_SETPWRM_PWR_OK 0x00
+#define GB_SVC_SETPWRM_PWR_LOCAL 0x01
 
 /*
  * All operation messages (both requests and responses) begin with

include/node.h

 #ifndef _NODE_H_
 #define _NODE_H_
 
+#include <zephyr/net/net_ip.h>
+
+struct gb_interface *node_find_by_addr(struct in6_addr *);
+
+struct gb_interface *node_create_interface(struct in6_addr *);
 
 #endif

include/operations.h

@@ -17,6 +17,13 @@
 #define E_NULL_REQUEST 1
 #define E_ALREADY_SENT 2
 
+struct gb_controller;
+
+typedef struct gb_message *(*gb_controller_read_callback_t)(
+    struct gb_controller *, uint16_t);
+typedef int (*gb_controller_write_callback_t)(struct gb_controller *,
+                                              struct gb_message *, uint16_t);
+
 /*
  * Struct to represent greybus message. This is a variable sized type.
  *
@@ -41,8 +48,8 @@ struct gb_message {
  * @param ctrl_data: private controller data
  */
 struct gb_controller {
-  struct gb_message *(*read)(struct gb_controller *, uint16_t);
-  int (*write)(struct gb_controller *, struct gb_message *, uint16_t);
+  gb_controller_read_callback_t read;
+  gb_controller_write_callback_t write;
   void *ctrl_data;
 };
 
@@ -55,6 +62,7 @@ struct gb_controller {
 struct gb_interface {
   uint8_t id;
   struct gb_controller controller;
+  sys_dnode_t node;
 };
 
 /*
@@ -171,4 +179,9 @@ struct gb_message *gb_message_request_alloc(const void *, size_t, uint8_t,
 struct gb_message *gb_message_response_alloc(const void *, size_t, uint8_t,
                                              uint16_t);
 
+struct gb_interface *gb_interface_alloc(gb_controller_read_callback_t,
+                                        gb_controller_write_callback_t, void *);
+
+void gb_interface_dealloc(struct gb_interface *);
+
 #endif

include/svc.h

@@ -33,4 +33,7 @@ struct gb_interface *svc_init();
  */
 bool svc_is_ready();
 
+
+int svc_send_module_inserted(uint8_t);
+
 #endif

src/main.c

@@ -6,10 +6,11 @@
 
 #include "ap.h"
 #include "hdlc.h"
-#include "node_handler.h"
-#include "node_table.h"
+// #include "node_handler.h"
+// #include "node_table.h"
 #include "operations.h"
 #include "svc.h"
+#include "node.h"
 #include <zephyr/sys/dlist.h>
 #include <stdbool.h>
 #include <zephyr/drivers/uart.h>
@@ -24,10 +25,6 @@
 #define UART_DEVICE_NODE DT_CHOSEN(zephyr_shell_uart)
 #define NODE_DISCOVERY_INTERVAL 5000
 #define MAX_GREYBUS_NODES CONFIG_BEAGLEPLAY_GREYBUS_MAX_NODES
-#define MAX_NUMBER_OF_SOCKETS CONFIG_NET_SOCKETS_POLL_MAX
-#define NODE_READER_INTERVAL 500
-#define NODE_WRITER_INTERVAL 500
-#define NODE_POLL_TIMEOUT 500
 
 LOG_MODULE_REGISTER(cc1352_greybus, CONFIG_BEAGLEPLAY_GREYBUS_LOG_LEVEL);
 
@@ -38,8 +35,8 @@ static sys_dlist_t gb_connections_list =
     SYS_DLIST_STATIC_INIT(&gb_connections_list);
 
 // Thread responsible for beagleconnect node discovery.
-// K_THREAD_DEFINE(node_discovery, 1024, node_discovery_entry, NULL, NULL, NULL, 5,
-//                 0, 0);
+K_THREAD_DEFINE(node_discovery, 1024, node_discovery_entry, NULL, NULL, NULL, 5,
+                0, 0);
 
 K_THREAD_DEFINE(apbridge, 2048, apbridge_entry, NULL, NULL, NULL, 5, 0,
                 0);
@@ -84,6 +81,12 @@ static int get_all_nodes(struct in6_addr *node_array,
 static void node_discovery_entry(void *p1, void *p2, void *p3) {
   int ret;
   struct in6_addr node_array[MAX_GREYBUS_NODES];
+  struct gb_interface *intf;
+
+  // Wait until SVC is ready
+  while(!svc_is_ready()) {
+    k_sleep(K_MSEC(1000));
+  }
 
   while (1) {
     ret = get_all_nodes(node_array, MAX_GREYBUS_NODES);
@@ -94,13 +97,10 @@ static void node_discovery_entry(void *p1, void *p2, void *p3) {
     // LOG_INF("Discoverd %u nodes", ret);
 
     for (size_t i = 0; i < ret; ++i) {
-      if (!node_table_is_active_by_addr(&node_array[i])) {
-        if (node_table_add_node(&node_array[i]) < 0) {
-          LOG_WRN("Failed to add node");
-        } else {
-          node_handler_setup_node_queue(&node_array[i], 0);
-          LOG_INF("Added Greybus Node");
-        }
+      intf = node_find_by_addr(&node_array[i]);
+      if (intf == NULL) {
+        intf = node_create_interface(&node_array[i]);
+        svc_send_module_inserted(intf->id);
       }
     }
 

src/node.c

+#include "greybus_protocol.h"
 #include "operations.h"
+#include "zephyr/sys/dlist.h"
 #include <zephyr/logging/log.h>
 #include <zephyr/net/socket.h>
 
 LOG_MODULE_DECLARE(cc1352_greybus, CONFIG_BEAGLEPLAY_GREYBUS_LOG_LEVEL);
 
+static sys_dlist_t node_interface_list =
+    SYS_DLIST_STATIC_INIT(&node_interface_list);
+
 static int write_data(int sock, const void *data, size_t len) {
   int ret;
   int transmitted = 0;
@@ -35,34 +40,121 @@ static int read_data(int sock, void *data, size_t len) {
   return recieved;
 }
 
+static struct gb_message *gb_message_receive(int sock, bool *flag) {
+  int ret;
+  struct gb_operation_msg_hdr hdr;
+  struct gb_message *msg;
+  size_t payload_size;
+
+  ret = read_data(sock, &hdr, sizeof(struct gb_operation_msg_hdr));
+  if (ret <= 0) {
+    *flag = ret == 0;
+    goto early_exit;
+  }
+
+  payload_size = hdr.size - sizeof(struct gb_operation_msg_hdr);
+  msg = k_malloc(sizeof(struct gb_message) + payload_size);
+  if (msg == NULL) {
+    LOG_ERR("Failed to allocate node message");
+    goto free_msg;
+  }
+
+  memcpy(&msg->header, &hdr, sizeof(struct gb_operation_msg_hdr));
+  msg->payload_size = payload_size;
+  ret = read_data(sock, msg->payload, msg->payload_size);
+  if (ret <= 0) {
+    *flag = ret == 0;
+    goto free_msg;
+  }
+
+  return msg;
+
+free_msg:
+  k_free(msg);
+early_exit:
+  return NULL;
+}
+
+static int gb_message_send(int sock, const struct gb_message *msg) {
+  int ret;
+
+  ret = write_data(sock, &msg->header, sizeof(struct gb_operation_msg_hdr));
+  if (ret < 0) {
+    return -1;
+  }
+
+  ret = write_data(sock, msg->payload, msg->payload_size);
+  if (ret < 0) {
+    return -1;
+  }
+
+  return SUCCESS;
+}
+
 struct node_control_data {
+  int *cports;
+  uint16_t cports_len;
+  struct in6_addr addr;
 };
 
-
 static struct gb_message *node_inf_read(struct gb_controller *ctrl,
-                                       uint16_t cport_id) {
+                                        uint16_t cport_id) {
+  struct zsock_pollfd fd[1];
+  int ret;
+  bool flag = false;
+  struct gb_message *msg = NULL;
+  struct node_control_data *ctrl_data = ctrl->ctrl_data;
+
+  if (cport_id >= ctrl_data->cports_len) {
+    goto early_exit;
+  }
+
+  fd[0].fd = ctrl_data->cports[cport_id];
+  fd[0].events = ZSOCK_POLLIN;
+
+  ret = zsock_poll(fd, 1, 0);
+  if (ret <= 0) {
+    goto early_exit;
+  }
+
+  if (fd[0].revents & ZSOCK_POLLIN) {
+    msg = gb_message_receive(fd[0].fd, &flag);
+    if (flag) {
+      LOG_ERR("Socket closed by Peer Node");
+    }
+  }
+
+early_exit:
   return NULL;
 }
 
 static int node_inf_write(struct gb_controller *ctrl, struct gb_message *msg,
-                         uint16_t cport_id) {
-  return -1;
-}
+                          uint16_t cport_id) {
+  struct node_control_data *ctrl_data = ctrl->ctrl_data;
+  if (cport_id >= ctrl_data->cports_len) {
+    return -1;
+  }
 
+  return gb_message_send(ctrl_data->cports[cport_id], msg);
+}
 
-struct gb_interface *node_create_interface() {
-  struct node_control_data *ctrl_data = k_malloc(sizeof(struct node_control_data));
+struct gb_interface *node_create_interface(struct in6_addr *addr) {
+  struct node_control_data *ctrl_data =
+      k_malloc(sizeof(struct node_control_data));
   if (ctrl_data == NULL) {
     return NULL;
   }
-  struct gb_interface *inf = k_malloc(sizeof(struct gb_interface));
+  ctrl_data->cports = NULL;
+  ctrl_data->cports_len = 0;
+  memcpy(&ctrl_data->addr, addr, sizeof(struct in6_addr));
+
+  struct gb_interface *inf =
+      gb_interface_alloc(node_inf_read, node_inf_write, ctrl_data);
   if (inf == NULL) {
     goto free_ctrl_data;
   }
 
-  inf->controller.ctrl_data = ctrl_data;
-  inf->controller.read = node_inf_read;
-  inf->controller.write = node_inf_write;
+  sys_dlist_append(&node_interface_list, &inf->node);
 
   return inf;
 
@@ -76,6 +168,33 @@ void node_destroy_interface(struct gb_interface *inf) {
     return;
   }
 
+  sys_dlist_remove(&inf->node);
   k_free(inf->controller.ctrl_data);
-  k_free(inf);
+  gb_interface_dealloc(inf);
+}
+
+struct gb_interface *node_find_by_id(uint8_t id) {
+  struct gb_interface *inf;
+
+  SYS_DLIST_FOR_EACH_CONTAINER(&node_interface_list, inf, node) {
+    if (inf->id == id) {
+      return inf;
+    }
+  }
+
+  return NULL;
+}
+
+struct gb_interface *node_find_by_addr(struct in6_addr *addr) {
+  struct gb_interface *inf;
+  struct node_control_data *ctrl_data;
+
+  SYS_DLIST_FOR_EACH_CONTAINER(&node_interface_list, inf, node) {
+    ctrl_data = inf->controller.ctrl_data;
+    if (memcmp(&ctrl_data->addr, addr, sizeof(struct in6_addr)) == 0) {
+      return inf;
+    }
+  }
+
+  return NULL;
 }

src/operations.c

@@ -12,12 +12,24 @@
 
 LOG_MODULE_DECLARE(cc1352_greybus, CONFIG_BEAGLEPLAY_GREYBUS_LOG_LEVEL);
 
-static atomic_t operation_id_counter = ATOMIC_INIT(1);
+#define OPERATION_ID_START 1
+#define INTERFACE_ID_START 2
+
+static atomic_t operation_id_counter = ATOMIC_INIT(OPERATION_ID_START);
+static atomic_t interface_id_counter = ATOMIC_INIT(INTERFACE_ID_START);
 
 static uint16_t new_operation_id() {
   atomic_val_t temp = atomic_inc(&operation_id_counter);
   if (temp == UINT16_MAX) {
-    atomic_set(&operation_id_counter, 1);
+    atomic_set(&operation_id_counter, OPERATION_ID_START);
+  }
+  return temp;
+}
+
+static uint8_t new_interface_id() {
+  atomic_val_t temp = atomic_inc(&interface_id_counter);
+  if (temp == UINT8_MAX) {
+    atomic_set(&interface_id_counter, INTERFACE_ID_START);
   }
   return temp;
 }
@@ -97,3 +109,24 @@ struct gb_message *gb_message_response_alloc(const void *payload,
   return gb_message_alloc(payload, payload_len, OP_RESPONSE | request_type,
                           operation_id);
 }
+
+struct gb_interface *gb_interface_alloc(gb_controller_read_callback_t read_cb,
+                                        gb_controller_write_callback_t write_cb,
+                                        void *ctrl_data) {
+  struct gb_interface *intf = k_malloc(sizeof(struct gb_interface));
+  if (intf == NULL) {
+    return NULL;
+  }
+
+  intf->id = new_interface_id();
+  intf->controller.read = read_cb;
+  intf->controller.write = write_cb;
+  intf->controller.ctrl_data = ctrl_data;
+  sys_dnode_init(&intf->node);
+
+  return intf;
+}
+
+void gb_interface_dealloc(struct gb_interface *intf) {
+  k_free(intf);
+}

src/svc.c

@@ -2,11 +2,11 @@
 #include "ap.h"
 #include "greybus_protocol.h"
 #include "operations.h"
-#include <zephyr/sys/atomic.h>
 #include <errno.h>
 #include <zephyr/kernel.h>
 #include <zephyr/logging/log.h>
 #include <zephyr/net/socket.h>
+#include <zephyr/sys/atomic.h>
 
 #define ENDO_ID 0x4755
 
@@ -20,6 +20,12 @@ struct svc_control_data {
 
 static struct svc_control_data svc_ctrl_data;
 
+struct gb_svc_module_inserted_request {
+  uint8_t primary_intf_id;
+  uint8_t intf_count;
+  uint16_t flags;
+} __packed;
+
 struct gb_svc_version_request {
   uint8_t major;
   uint8_t minor;
@@ -115,11 +121,6 @@ static void svc_version_response_handler(struct gb_message *msg) {
   svc_send_hello();
 }
 
-static void svc_ping_response_handler(struct gb_message *msg) {
-  ARG_UNUSED(msg);
-  LOG_DBG("Received Pong");
-}
-
 static void svc_hello_response_handler(struct gb_message *msg) {
   LOG_DBG("Hello Response Success");
   atomic_set_bit(svc_is_read_flag, 0);
@@ -137,17 +138,17 @@ static void svc_pwrm_get_rail_count_handler(struct gb_message *msg) {
 
 static void svc_intf_set_pwrm_handler(struct gb_message *msg) {
   uint8_t tx_mode, rx_mode;
-  struct gb_svc_intf_set_pwrm_response resp = {
-    .result_code = GB_SVC_SETPWRM_PWR_LOCAL
-  };
-  struct gb_svc_intf_set_pwrm_request *req = (struct gb_svc_intf_set_pwrm_request *)msg->payload;
+  struct gb_svc_intf_set_pwrm_response resp = {.result_code =
+                                                   GB_SVC_SETPWRM_PWR_LOCAL};
+  struct gb_svc_intf_set_pwrm_request *req =
+      (struct gb_svc_intf_set_pwrm_request *)msg->payload;
   tx_mode = req->tx_mode;
   rx_mode = req->rx_mode;
 
   if (tx_mode == GB_SVC_UNIPRO_HIBERNATE_MODE &&
       rx_mode == GB_SVC_UNIPRO_HIBERNATE_MODE) {
     resp.result_code = GB_SVC_SETPWRM_PWR_OK;
-  } 
+  }
 
   svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_set_pwrm_response));
 }
@@ -172,11 +173,14 @@ static void gb_handle_msg(struct gb_message *msg) {
     svc_version_response_handler(msg);
     break;
   case GB_SVC_TYPE_PING_RESPONSE:
-    svc_ping_response_handler(msg);
+    LOG_DBG("Received Pong");
     break;
   case GB_SVC_TYPE_HELLO_RESPONSE:
     svc_hello_response_handler(msg);
     break;
+  case GB_SVC_TYPE_MODULE_INSERTED_RESPONSE:
+    LOG_DBG("Successful Module Inserted Response");
+    break;
   default:
     LOG_WRN("Handling SVC operation Type %X not supported yet",
             msg->header.type);
@@ -207,6 +211,13 @@ struct gb_interface *svc_init() {
   return &intf;
 }
 
-bool svc_is_ready() {
-  return atomic_test_bit(svc_is_read_flag, 0);
+bool svc_is_ready() { return atomic_test_bit(svc_is_read_flag, 0); }
+
+int svc_send_module_inserted(uint8_t primary_intf_id) {
+  struct gb_svc_module_inserted_request req = {
+      .primary_intf_id = primary_intf_id, .intf_count = 1, .flags = 0};
+
+  return control_send_request(&req,
+                              sizeof(struct gb_svc_module_inserted_request),
+                              GB_SVC_TYPE_MODULE_INSERTED_REQUEST);
 }