diff --git a/controller.h b/controller.h
index 69432e8879e832f79288f753fcdda8493c95a239..dfcdf2427f2afb73ae819c5962ea6c90dcede551 100644
--- a/controller.h
+++ b/controller.h
@@ -48,6 +48,8 @@ struct interface {
struct controller *ctrl;
TAILQ_ENTRY(interface) node;
pthread_t thread;
+
+ struct greybus_driver *gb_drivers[GB_NETLINK_NUM_CPORT];
};
struct controller {
diff --git a/gbridge.h b/gbridge.h
index 608acbdc89bac3f7fe925a943457086cefa242e7..07f9466dba3dfc9a9b51ce37d5fbdbd0d97396a1 100644
--- a/gbridge.h
+++ b/gbridge.h
@@ -32,6 +32,7 @@
#include <gb_netlink.h>
#define SVC_CPORT 0
+#define AP_INTF_ID 0
#define OP_RESPONSE 0x80
#ifndef TAILQ_FOREACH_SAFE
@@ -123,9 +124,11 @@ int greybus_init(void);
struct operation *greybus_alloc_operation(uint8_t type,
void *payload, size_t len);
int greybus_alloc_response(struct operation *op, size_t size);
-int greybus_register_driver(uint16_t cport_id,
+int greybus_register_driver(uint8_t intf_id, uint16_t cport_id,
struct greybus_driver *driver);
-int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr);
-int greybus_send_request(uint16_t cport_id, struct operation *op);
+int greybus_handler(uint8_t intf_id, uint16_t cport_id,
+ struct gb_operation_msg_hdr *hdr);
+int greybus_send_request(uint8_t intf_id, uint16_t cport_id,
+ struct operation *op);
#endif /* _GBRIDGE_H_ */
diff --git a/greybus.c b/greybus.c
index c7934c8951ab81a7ac315547a039972abbd6f0bd..1afa19ceb820a55b284d0ae302fdf6b09c372a6f 100644
--- a/greybus.c
+++ b/greybus.c
@@ -26,9 +26,9 @@
#include "gbridge.h"
#include "netlink.h"
+#include "controller.h"
-static struct greybus_driver *gb_drivers[GB_NETLINK_NUM_CPORT];
-static TAILQ_HEAD(head, operation) operations;
+static TAILQ_HEAD(operation_head, operation) operations;
enum gb_operation_result {
GB_OP_SUCCESS = 0x00,
@@ -157,7 +157,8 @@ static void greybus_free_operation(struct operation *op)
free(op);
}
-int greybus_send_request(uint16_t cport_id, struct operation *op)
+int greybus_send_request(uint8_t intf_id, uint16_t cport_id,
+ struct operation *op)
{
int len;
int ret;
@@ -174,7 +175,8 @@ int greybus_send_request(uint16_t cport_id, struct operation *op)
return 0;
}
-static int greybus_send_response(uint16_t cport_id, struct operation *op)
+static int greybus_send_response(uint8_t intf_id, uint16_t cport_id,
+ struct operation *op)
{
int len;
int ret;
@@ -238,14 +240,22 @@ int _greybus_handler(struct greybus_driver *driver, struct operation *op)
return handler->callback(op);
}
-int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr)
+int greybus_handler(uint8_t intf2_id, uint16_t cport_id,
+ struct gb_operation_msg_hdr *hdr)
{
int ret;
struct operation *op;
+ struct interface *intf2;
pr_dump(hdr, gb_operation_msg_size(hdr));
- if (!gb_drivers[cport_id]) {
+ intf2 = get_interface(intf2_id);
+ if (!intf2) {
+ pr_err("Invalid interface id %d\n", intf2_id);
+ return -EINVAL;
+ }
+
+ if (!intf2->gb_drivers[cport_id]) {
pr_err("No driver registered for cport %d\n", cport_id);
return -EINVAL;
}
@@ -260,13 +270,13 @@ int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr)
TAILQ_REMOVE(&operations, op, cnode);
if (_greybus_alloc_response(op, hdr))
return -ENOMEM;
- ret = _greybus_handler(gb_drivers[cport_id], op);
+ ret = _greybus_handler(intf2->gb_drivers[cport_id], op);
} else {
op = _greybus_alloc_operation(hdr);
if (!op)
return -ENOMEM;
- ret = _greybus_handler(gb_drivers[cport_id], op);
+ ret = _greybus_handler(intf2->gb_drivers[cport_id], op);
if (!op->resp) {
if (greybus_alloc_response(op, 0)) {
pr_err("Failed to alloc greybus response\n");
@@ -276,7 +286,7 @@ int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr)
}
op->resp->result = greybus_errno_to_result(ret);
- ret = greybus_send_response(cport_id, op);
+ ret = greybus_send_response(intf2_id, cport_id, op);
}
free_op:
@@ -285,19 +295,26 @@ int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr)
return ret;
}
-int greybus_register_driver(uint16_t cport_id,
+int greybus_register_driver(uint8_t intf_id, uint16_t cport_id,
struct greybus_driver *driver)
{
int i = 0;
int id = 0;
int id_min = -1;
+ struct interface *intf;
+
+ intf = get_interface(intf_id);
+ if (!intf) {
+ pr_err("Invalid interface id %d\n", intf_id);
+ return -EINVAL;
+ }
if (cport_id >= GB_NETLINK_NUM_CPORT) {
pr_err("Invalid cport id %d\n", cport_id);
return -EINVAL;
}
- if (gb_drivers[cport_id]) {
+ if (intf->gb_drivers[cport_id]) {
pr_err("A driver has already been registered for cport id %d\n",
cport_id);
return -EINVAL;
@@ -318,7 +335,7 @@ int greybus_register_driver(uint16_t cport_id,
id++;
}
- gb_drivers[cport_id] = driver;
+ intf->gb_drivers[cport_id] = driver;
return 0;
}
@@ -326,8 +343,6 @@ int greybus_register_driver(uint16_t cport_id,
int greybus_init(void)
{
TAILQ_INIT(&operations);
- memset(gb_drivers, 0,
- sizeof(struct operation_handler *) * GB_NETLINK_NUM_CPORT);
- return svc_register_driver();
+ return 0;
}
diff --git a/netlink.c b/netlink.c
index 4a9da661bac1c8d27d153cb8266e676a4e7fe6b4..43c8a320a834b05ba14032a43846eb4a7e52d6cc 100644
--- a/netlink.c
+++ b/netlink.c
@@ -40,6 +40,8 @@ parse_gb_nl_msg(struct nl_cache_ops *unused, struct genl_cmd *cmd,
{
struct gb_operation_msg_hdr *hdr;
uint16_t hd_cport_id;
+ uint16_t cport_id;
+ uint8_t intf_id;
size_t len;
int ret;
@@ -56,7 +58,8 @@ parse_gb_nl_msg(struct nl_cache_ops *unused, struct genl_cmd *cmd,
}
if (hd_cport_id == SVC_CPORT) {
- ret = greybus_handler(hd_cport_id, hdr);
+ hd_to_intf_cport_id(hd_cport_id, &intf_id, &cport_id);
+ ret = greybus_handler(intf_id, cport_id, hdr);
if (ret) {
pr_err("Failed to handle svc operation %d: %d\n",
hdr->type, ret);
diff --git a/protocols/svc.c b/protocols/svc.c
index 4fb176ef26903b6e62d51d1493e3a892656a61c0..48f49129f34de036a03477753ef626792b6be1d8 100644
--- a/protocols/svc.c
+++ b/protocols/svc.c
@@ -31,7 +31,6 @@
/* TODO: Can we use other IDs ? */
#define ENDO_ID 0x4755
-#define AP_INTF_ID 0x0
static int svc_send_hello_request(void);
@@ -366,7 +365,7 @@ static struct greybus_driver svc_driver = {
};
int svc_register_driver(void) {
- return greybus_register_driver(0, &svc_driver);
+ return greybus_register_driver(AP_INTF_ID, SVC_CPORT, &svc_driver);
}
int svc_send_protocol_version_request(void)
@@ -382,7 +381,7 @@ int svc_send_protocol_version_request(void)
if (!op)
return -ENOMEM;
- return greybus_send_request(0, op);
+ return greybus_send_request(AP_INTF_ID, SVC_CPORT, op);
}
int svc_send_hello_request(void)
@@ -397,7 +396,7 @@ int svc_send_hello_request(void)
if (!op)
return -ENOMEM;
- return greybus_send_request(0, op);
+ return greybus_send_request(AP_INTF_ID, SVC_CPORT, op);
}
int svc_send_module_inserted_event(uint8_t intf_id,
@@ -415,7 +414,7 @@ int svc_send_module_inserted_event(uint8_t intf_id,
if (!op)
return -ENOMEM;
- return greybus_send_request(0, op);
+ return greybus_send_request(AP_INTF_ID, SVC_CPORT, op);
}
int svc_init(void)