diff --git a/include/greybus_protocol.h b/include/greybus_protocol.h
index 7c00b295f772626800dd708d31f1c460c383dba6..b4d7915ac3a8630f3708cc03df9b084111adab14 100644
--- a/include/greybus_protocol.h
+++ b/include/greybus_protocol.h
@@ -26,6 +26,9 @@
#define GB_SVC_TYPE_CONN_CREATE_REQUEST 0x07
#define GB_SVC_TYPE_CONN_CREATE_RESPONSE \
OP_RESPONSE | GB_SVC_TYPE_CONN_CREATE_REQUEST
+#define GB_SVC_TYPE_CONN_DESTROY_REQUEST 0x08
+#define GB_SVC_TYPE_CONN_DESTROY_RESPONSE \
+ OP_RESPONSE | GB_SVC_TYPE_CONN_DESTROY_REQUEST
#define GB_SVC_TYPE_DME_PEER_GET_REQUEST 0x09
#define GB_SVC_TYPE_DME_PEER_GET_RESPONSE \
OP_RESPONSE | GB_SVC_TYPE_DME_PEER_GET_REQUEST
@@ -70,6 +73,9 @@
#define GB_SVC_TYPE_INTF_ACTIVATE_REQUEST 0x27
#define GB_SVC_TYPE_INTF_ACTIVATE_RESPONSE \
OP_RESPONSE | GB_SVC_TYPE_INTF_ACTIVATE_REQUEST
+#define GB_SVC_TYPE_INTF_RESUME_REQUEST 0x28
+#define GB_SVC_TYPE_INTF_RESUME_RESPONSE \
+ OP_RESPONSE | GB_SVC_TYPE_INTF_RESUME_REQUEST
#define GB_SVC_UNIPRO_HIBERNATE_MODE 0x11
diff --git a/include/operations.h b/include/operations.h
index 5ea05bd71713cce341f3a69846298cf1ed937f34..83013ddc0306eae234cac0f38362f4548fa4e739 100644
--- a/include/operations.h
+++ b/include/operations.h
@@ -25,6 +25,8 @@ typedef int (*gb_controller_write_callback_t)(struct gb_controller *,
struct gb_message *, uint16_t);
typedef int (*gb_controller_create_connection_t)(struct gb_controller *,
uint16_t);
+typedef void (*gb_controller_destroy_connection_t)(struct gb_controller *,
+ uint16_t);
/*
* Struct to represent greybus message. This is a variable sized type.
@@ -53,6 +55,7 @@ struct gb_controller {
gb_controller_read_callback_t read;
gb_controller_write_callback_t write;
gb_controller_create_connection_t create_connection;
+ gb_controller_destroy_connection_t destroy_connection;
void *ctrl_data;
};
@@ -185,10 +188,14 @@ struct gb_message *gb_message_response_alloc(const void *, size_t, uint8_t,
struct gb_interface *gb_interface_alloc(gb_controller_read_callback_t,
gb_controller_write_callback_t,
gb_controller_create_connection_t,
+ gb_controller_destroy_connection_t,
void *);
void gb_interface_dealloc(struct gb_interface *);
sys_dlist_t *gb_connections_list_get();
+void gb_destroy_connection(struct gb_interface *, struct gb_interface *,
+ uint16_t, uint16_t);
+
#endif
diff --git a/src/ap.c b/src/ap.c
index 04512f1331e4666be46a0722fdfb7b5869374ae6..c097cb36676be6f1ace4bb041fc0c763844f6400 100644
--- a/src/ap.c
+++ b/src/ap.c
@@ -32,6 +32,21 @@ static int ap_inf_create_connection(struct gb_controller *ctrl,
}
}
+static void ap_inf_destroy_connection(struct gb_controller *ctrl,
+ uint16_t cport_id) {
+ struct gb_message *msg;
+ struct ap_controller_data *ctrl_data = ctrl->ctrl_data;
+ if (cport_id >= AP_MAX_NODES) {
+ return;
+ }
+
+ msg = k_fifo_get(&ctrl_data->pending_read[cport_id], K_NO_WAIT);
+ while (msg) {
+ gb_message_dealloc(msg);
+ msg = k_fifo_get(&ctrl_data->pending_read[cport_id], K_NO_WAIT);
+ }
+}
+
static struct ap_controller_data ap_ctrl_data;
static struct gb_interface intf = {
@@ -40,6 +55,7 @@ static struct gb_interface intf = {
.write = ap_inf_write,
.read = ap_inf_read,
.create_connection = ap_inf_create_connection,
+ .destroy_connection = ap_inf_destroy_connection,
.ctrl_data = &ap_ctrl_data,
}};
diff --git a/src/node.c b/src/node.c
index 14ac953534a0a8f12ff18e8a3858d36866d26f4b..200f1ef299f6c1bd58bacdda3298e9836e240c8d 100644
--- a/src/node.c
+++ b/src/node.c
@@ -174,6 +174,19 @@ static int node_intf_create_connection(struct gb_controller *ctrl,
return ret;
}
+static void node_intf_destroy_connection(struct gb_controller *ctrl,
+ uint16_t cport_id) {
+ struct node_control_data *ctrl_data = ctrl->ctrl_data;
+
+ if (cport_id >= ctrl_data->cports_len) {
+ return;
+ }
+
+ zsock_close(ctrl_data->cports[cport_id]);
+
+ ctrl_data->cports[cport_id] = -1;
+}
+
static struct gb_message *node_inf_read(struct gb_controller *ctrl,
uint16_t cport_id) {
struct zsock_pollfd fd[1];
@@ -226,7 +239,8 @@ struct gb_interface *node_create_interface(struct in6_addr *addr) {
memcpy(&ctrl_data->addr, addr, sizeof(struct in6_addr));
struct gb_interface *inf = gb_interface_alloc(
- node_inf_read, node_inf_write, node_intf_create_connection, ctrl_data);
+ node_inf_read, node_inf_write, node_intf_create_connection,
+ node_intf_destroy_connection, ctrl_data);
if (inf == NULL) {
goto free_ctrl_data;
}
diff --git a/src/operations.c b/src/operations.c
index 948972ad7dbd933adb89841817b0f9794f6668eb..8463811f34aad9d1e7c683ea3be3ced407aef113 100644
--- a/src/operations.c
+++ b/src/operations.c
@@ -93,6 +93,37 @@ struct gb_connection *gb_create_connection(struct gb_interface *inf_ap,
return conn;
}
+static struct gb_connection *gb_connection_get(struct gb_interface *inf_ap,
+ struct gb_interface *inf_peer) {
+ struct gb_connection *conn;
+
+ SYS_DLIST_FOR_EACH_CONTAINER(&gb_connections_list, conn, node) {
+ // While the names are inf_peer and inf_ap, they are just arbitrary. So do
+ // comparisons in reverse as well
+ if ((conn->inf_peer == inf_peer && conn->inf_ap == inf_ap) ||
+ (conn->inf_peer == inf_ap && conn->inf_ap == inf_peer)) {
+ return conn;
+ }
+ }
+
+ return NULL;
+}
+
+void gb_destroy_connection(struct gb_interface *inf_ap,
+ struct gb_interface *inf_peer, uint16_t ap_cport,
+ uint16_t peer_cport) {
+ struct gb_connection *conn = gb_connection_get(inf_ap, inf_peer);
+
+ sys_dlist_remove(&conn->node);
+
+ conn->inf_ap->controller.destroy_connection(&conn->inf_ap->controller,
+ ap_cport);
+ conn->inf_peer->controller.destroy_connection(&conn->inf_peer->controller,
+ peer_cport);
+
+ k_free(conn);
+}
+
sys_dlist_t *gb_connections_list_get() { return &gb_connections_list; }
static struct gb_message *
@@ -137,6 +168,7 @@ struct gb_interface *
gb_interface_alloc(gb_controller_read_callback_t read_cb,
gb_controller_write_callback_t write_cb,
gb_controller_create_connection_t create_connection,
+ gb_controller_destroy_connection_t destroy_connection,
void *ctrl_data) {
struct gb_interface *intf = k_malloc(sizeof(struct gb_interface));
if (intf == NULL) {
@@ -147,6 +179,7 @@ gb_interface_alloc(gb_controller_read_callback_t read_cb,
intf->controller.read = read_cb;
intf->controller.write = write_cb;
intf->controller.create_connection = create_connection;
+ intf->controller.destroy_connection = destroy_connection;
intf->controller.ctrl_data = ctrl_data;
sys_dnode_init(&intf->node);
diff --git a/src/svc.c b/src/svc.c
index 4476defd8a3c786ca2e5e3c125d2cf08990bb9ee..90c74d961adb45f51f56a8c8424e1cb59ff23eb1 100644
--- a/src/svc.c
+++ b/src/svc.c
@@ -29,13 +29,41 @@ static int svc_inf_create_connection(struct gb_controller *ctrl,
return cport_id == 0 && !svc_is_ready();
}
+static void svc_inf_destroy_connection(struct gb_controller *ctrl,
+ uint16_t cport_id) {
+
+ struct gb_message *msg;
+
+ // Set svc to uninitialized
+ atomic_set_bit_to(svc_is_read_flag, 0, false);
+
+ msg = k_fifo_get(&svc_ctrl_data.pending_read, K_NO_WAIT);
+ while (msg) {
+ // Free all pending messages
+ gb_message_dealloc(msg);
+ msg = k_fifo_get(&svc_ctrl_data.pending_read, K_NO_WAIT);
+ }
+}
+
static struct gb_interface intf = {
.id = SVC_INF_ID,
.controller = {.read = svc_inf_read,
.write = svc_inf_write,
.create_connection = svc_inf_create_connection,
+ .destroy_connection = svc_inf_destroy_connection,
.ctrl_data = &svc_ctrl_data}};
+struct gb_svc_intf_resume_response {
+ uint8_t status;
+} __packed;
+
+struct gb_svc_conn_destroy_request {
+ uint8_t intf1_id;
+ uint16_t cport1_id;
+ uint8_t intf2_id;
+ uint16_t cport2_id;
+} __packed;
+
struct gb_svc_conn_create_request {
uint8_t intf1_id;
uint16_t cport1_id;
@@ -184,7 +212,8 @@ static void svc_empty_request_handler(struct gb_message *msg) {
static void svc_pwrm_get_rail_count_handler(struct gb_message *msg) {
struct gb_svc_pwrmon_rail_count_get_response req = {.rail_count = 0};
svc_response_helper(msg, &req,
- sizeof(struct gb_svc_pwrmon_rail_count_get_response), GB_SVC_OP_SUCCESS);
+ sizeof(struct gb_svc_pwrmon_rail_count_get_response),
+ GB_SVC_OP_SUCCESS);
}
static void svc_intf_set_pwrm_handler(struct gb_message *msg) {
@@ -201,44 +230,51 @@ static void svc_intf_set_pwrm_handler(struct gb_message *msg) {
resp.result_code = GB_SVC_SETPWRM_PWR_OK;
}
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_set_pwrm_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_set_pwrm_response),
+ GB_SVC_OP_SUCCESS);
}
static void svc_intf_vsys_enable_disable_handler(struct gb_message *msg) {
struct gb_svc_intf_vsys_response resp = {.result_code = GB_SVC_INTF_VSYS_OK};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_vsys_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_vsys_response),
+ GB_SVC_OP_SUCCESS);
}
static void
svc_interface_refclk_enable_disable_handler(struct gb_message *msg) {
struct gb_svc_intf_refclk_response resp = {.result_code =
GB_SVC_INTF_REFCLK_OK};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_refclk_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_refclk_response),
+ GB_SVC_OP_SUCCESS);
}
static void
svc_interface_unipro_enable_disable_handler(struct gb_message *msg) {
struct gb_svc_intf_unipro_response resp = {.result_code =
GB_SVC_INTF_UNIPRO_OK};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_unipro_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_unipro_response),
+ GB_SVC_OP_SUCCESS);
}
static void svc_interface_activate_handler(struct gb_message *msg) {
struct gb_svc_intf_activate_response resp = {
.status = GB_SVC_OP_SUCCESS, .intf_type = GB_SVC_INTF_TYPE_GREYBUS};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_activate_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_activate_response),
+ GB_SVC_OP_SUCCESS);
}
static void svc_dme_peer_get_handler(struct gb_message *msg) {
struct gb_svc_dme_peer_get_response resp = {.result_code = 0,
.attr_value = 0x0126};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_dme_peer_get_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_dme_peer_get_response),
+ GB_SVC_OP_SUCCESS);
}
static void svc_dme_peer_set_handler(struct gb_message *msg) {
struct gb_svc_dme_peer_set_response resp = {.result_code = 0};
- svc_response_helper(msg, &resp, sizeof(struct gb_svc_dme_peer_set_response), GB_SVC_OP_SUCCESS);
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_dme_peer_set_response),
+ GB_SVC_OP_SUCCESS);
}
static struct gb_interface *get_interface(uint8_t intf_id) {
@@ -269,6 +305,24 @@ static void svc_connection_create_handler(struct gb_message *msg) {
}
}
+static void svc_connection_destroy_handler(struct gb_message *msg) {
+ struct gb_svc_conn_destroy_request *req =
+ (struct gb_svc_conn_destroy_request *)msg->payload;
+ struct gb_interface *intf_1, *intf_2;
+
+ intf_1 = get_interface(req->intf1_id);
+ intf_2 = get_interface(req->intf2_id);
+
+ gb_destroy_connection(intf_1, intf_2, req->cport1_id, req->cport2_id);
+}
+
+static void svc_interface_resume_handler(struct gb_message *msg) {
+ struct gb_svc_intf_resume_response resp = {.status =
+ GB_SVC_INTF_TYPE_GREYBUS};
+ svc_response_helper(msg, &resp, sizeof(struct gb_svc_intf_resume_response),
+ GB_OP_SUCCESS);
+}
+
static void gb_handle_msg(struct gb_message *msg) {
LOG_DBG("Process SVC Operation %u of type %X", msg->header.id,
msg->header.type);
@@ -280,23 +334,11 @@ static void gb_handle_msg(struct gb_message *msg) {
case GB_SVC_TYPE_PING_REQUEST:
svc_empty_request_handler(msg);
break;
- case GB_SVC_TYPE_INTF_ACTIVATE_REQUEST:
- svc_interface_activate_handler(msg);
- break;
case GB_SVC_TYPE_CONN_CREATE_REQUEST:
svc_connection_create_handler(msg);
break;
- case GB_SVC_TYPE_INTF_UNIPRO_ENABLE_REQUEST:
- case GB_SVC_TYPE_INTF_UNIPRO_DISABLE_REQUEST:
- svc_interface_unipro_enable_disable_handler(msg);
- break;
- case GB_SVC_TYPE_INTF_REFCLK_ENABLE_REQUEST:
- case GB_SVC_TYPE_INTF_REFCLK_DISABLE_REQUEST:
- svc_interface_refclk_enable_disable_handler(msg);
- break;
- case GB_SVC_TYPE_INTF_VSYS_ENABLE_REQUEST:
- case GB_SVC_TYPE_INTF_VSYS_DISABLE_REQUEST:
- svc_intf_vsys_enable_disable_handler(msg);
+ case GB_SVC_TYPE_CONN_DESTROY_REQUEST:
+ svc_connection_destroy_handler(msg);
break;
case GB_SVC_TYPE_DME_PEER_GET_REQUEST:
svc_dme_peer_get_handler(msg);
@@ -304,11 +346,29 @@ static void gb_handle_msg(struct gb_message *msg) {
case GB_SVC_TYPE_DME_PEER_SET_REQUEST:
svc_dme_peer_set_handler(msg);
break;
+ case GB_SVC_TYPE_INTF_SET_PWRM_REQUEST:
+ svc_intf_set_pwrm_handler(msg);
+ break;
case GB_SVC_TYPE_PWRMON_RAIL_COUNT_GET_REQUEST:
svc_pwrm_get_rail_count_handler(msg);
break;
- case GB_SVC_TYPE_INTF_SET_PWRM_REQUEST:
- svc_intf_set_pwrm_handler(msg);
+ case GB_SVC_TYPE_INTF_VSYS_ENABLE_REQUEST:
+ case GB_SVC_TYPE_INTF_VSYS_DISABLE_REQUEST:
+ svc_intf_vsys_enable_disable_handler(msg);
+ break;
+ case GB_SVC_TYPE_INTF_REFCLK_ENABLE_REQUEST:
+ case GB_SVC_TYPE_INTF_REFCLK_DISABLE_REQUEST:
+ svc_interface_refclk_enable_disable_handler(msg);
+ break;
+ case GB_SVC_TYPE_INTF_UNIPRO_ENABLE_REQUEST:
+ case GB_SVC_TYPE_INTF_UNIPRO_DISABLE_REQUEST:
+ svc_interface_unipro_enable_disable_handler(msg);
+ break;
+ case GB_SVC_TYPE_INTF_ACTIVATE_REQUEST:
+ svc_interface_activate_handler(msg);
+ break;
+ case GB_SVC_TYPE_INTF_RESUME_REQUEST:
+ svc_interface_resume_handler(msg);
break;
case GB_SVC_TYPE_PROTOCOL_VERSION_RESPONSE:
svc_version_response_handler(msg);