From d56fbdb350f607e9673d931faad2e172928fb41c Mon Sep 17 00:00:00 2001
From: Ayush Singh <ayushsingh1325@gmail.com>
Date: Tue, 11 Jul 2023 23:46:21 +0530
Subject: [PATCH] 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 | 7 +-
include/node.h | 5 ++
include/operations.h | 17 ++++-
include/svc.h | 3 +
src/main.c | 30 ++++----
src/node.c | 143 +++++++++++++++++++++++++++++++++----
src/operations.c | 37 +++++++++-
src/svc.c | 39 ++++++----
8 files changed, 234 insertions(+), 47 deletions(-)
diff --git a/include/greybus_protocol.h b/include/greybus_protocol.h
index cb3f44e..fe6004c 100644
--- a/include/greybus_protocol.h
+++ b/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
diff --git a/include/node.h b/include/node.h
index 7c7f64f..deade08 100644
--- a/include/node.h
+++ b/include/node.h
@@ -1,5 +1,10 @@
#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
diff --git a/include/operations.h b/include/operations.h
index 5654458..b8b3d76 100644
--- a/include/operations.h
+++ b/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
diff --git a/include/svc.h b/include/svc.h
index e1c6417..ee2bce9 100644
--- a/include/svc.h
+++ b/include/svc.h
@@ -33,4 +33,7 @@ struct gb_interface *svc_init();
*/
bool svc_is_ready();
+
+int svc_send_module_inserted(uint8_t);
+
#endif
diff --git a/src/main.c b/src/main.c
index cef6a23..46b0799 100644
--- a/src/main.c
+++ b/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);
}
}
diff --git a/src/node.c b/src/node.c
index e4b880d..126f213 100644
--- a/src/node.c
+++ b/src/node.c
@@ -1,9 +1,14 @@
+#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;
}
diff --git a/src/operations.c b/src/operations.c
index 9132491..6f9c731 100644
--- a/src/operations.c
+++ b/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);
+}
diff --git a/src/svc.c b/src/svc.c
index 3700749..a3e65eb 100644
--- a/src/svc.c
+++ b/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);
}
--
GitLab