diff --git a/Makefile.am b/Makefile.am
index d0c4ead0c18e53416776e71a64265b4f84af253b..8409a40441c4e0bba8114284cb6a4638e38e0091 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -5,4 +5,6 @@ gbridge_CFLAGS += `pkg-config --cflags libnl-3.0 libnl-genl-3.0`
gbridge_SOURCES = main.c \
netlink.c \
- debug.c
\ No newline at end of file
+ debug.c \
+ greybus.c \
+ svc.c
\ No newline at end of file
diff --git a/gbridge.h b/gbridge.h
index b820da0658b3e52f2dc618ebbc7f7df1736fc284..6f66667f90273121e5d7ede6657b5010e525fd7d 100644
--- a/gbridge.h
+++ b/gbridge.h
@@ -25,14 +25,42 @@
#define __packed __attribute__((__packed__))
-/* Include kernel headers */
#include <greybus.h>
#include <greybus_protocols.h>
#include <gb_netlink.h>
#define SVC_CPORT 0
+#define OP_RESPONSE 0x80
+
+struct operation {
+ struct gb_operation_msg_hdr *req;
+ struct gb_operation_msg_hdr *resp;
+ uint16_t cport_id;
+ TAILQ_ENTRY(operation) cnode;
+};
+
+typedef int (*greybus_handler_t) (struct operation *);
+
+#define operation_to_request(op) \
+ (void *)((op)->req + 1)
+
+#define operation_to_response(op) \
+ (void *)((op)->resp + 1)
#define gb_operation_msg_size(hdr) \
le16toh(((struct gb_operation_msg_hdr *)(hdr))->size)
+int svc_init(void);
+int svc_handler(struct operation *op);
+int svc_send_intf_hotplug_event(uint8_t intf_id,
+ uint32_t vendor_id,
+ uint32_t product_id, uint64_t serial_number);
+
+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_send_request(uint16_t cport_id, struct operation *op);
+int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr);
+
#endif /* _GBRIDGE_H_ */
diff --git a/greybus.c b/greybus.c
new file mode 100644
index 0000000000000000000000000000000000000000..8dd8abb798a6346881dcc920a3f3b5b38df18a75
--- /dev/null
+++ b/greybus.c
@@ -0,0 +1,271 @@
+/*
+ * GBridge (Greybus Bridge)
+ * Copyright (c) 2016 Alexandre Bailon
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <errno.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <debug.h>
+#include <gb_netlink.h>
+
+#include "gbridge.h"
+#include "netlink.h"
+
+static greybus_handler_t gb_handlers[GB_NETLINK_NUM_CPORT];
+static TAILQ_HEAD(head, operation) operations;
+
+enum gb_operation_result {
+ GB_OP_SUCCESS = 0x00,
+ GB_OP_INTERRUPTED = 0x01,
+ GB_OP_TIMEOUT = 0x02,
+ GB_OP_NO_MEMORY = 0x03,
+ GB_OP_PROTOCOL_BAD = 0x04,
+ GB_OP_OVERFLOW = 0x05,
+ GB_OP_INVALID = 0x06,
+ GB_OP_RETRY = 0x07,
+ GB_OP_NONEXISTENT = 0x08,
+ GB_OP_UNKNOWN_ERROR = 0xfe,
+ GB_OP_INTERNAL = 0xff,
+};
+
+uint8_t greybus_errno_to_result(int err)
+{
+ switch (err) {
+ case 0:
+ return GB_OP_SUCCESS;
+ case -ENOMEM:
+ return GB_OP_NO_MEMORY;
+ case -EINTR:
+ return GB_OP_INTERRUPTED;
+ case -ETIMEDOUT:
+ return GB_OP_TIMEOUT;
+ case -EPROTO:
+ case -ENOSYS:
+ return GB_OP_PROTOCOL_BAD;
+ case -EINVAL:
+ return GB_OP_INVALID;
+ case -EOVERFLOW:
+ return GB_OP_OVERFLOW;
+ case -ENODEV:
+ case -ENXIO:
+ return GB_OP_NONEXISTENT;
+ case -EBUSY:
+ return GB_OP_RETRY;
+ default:
+ return GB_OP_UNKNOWN_ERROR;
+ }
+}
+
+static struct operation *_greybus_alloc_operation(struct gb_operation_msg_hdr
+ *hdr)
+{
+ struct operation *op;
+
+ op = malloc(sizeof(*op));
+ if (!op)
+ return NULL;
+
+ op->req = malloc(gb_operation_msg_size(hdr));
+ if (!op->req) {
+ free(op);
+ return NULL;
+ }
+
+ op->resp = NULL;
+ memcpy(op->req, hdr, gb_operation_msg_size(hdr));
+
+ return op;
+}
+
+struct operation *greybus_alloc_operation(uint8_t type,
+ void *payload, size_t len)
+{
+ uint16_t id = 0;
+ struct operation *op;
+
+ op = malloc(sizeof(*op));
+ if (!op)
+ return NULL;
+
+ op->req = malloc(sizeof(*op->req) + len);
+ if (!op->req) {
+ free(op);
+ return NULL;
+ }
+
+ if (++id == 0)
+ ++id;
+
+ op->resp = NULL;
+ op->req->type = type;
+ op->req->size = htole16(sizeof(*op->req) + len);
+ op->req->operation_id = htole16(id);
+ memcpy(op->req + 1, payload, len);
+
+ return op;
+}
+
+static int _greybus_alloc_response(struct operation *op,
+ struct gb_operation_msg_hdr *hdr)
+{
+ op->resp = malloc(gb_operation_msg_size(hdr));
+ if (!op->resp)
+ return -ENOMEM;
+
+ memcpy(op->resp, hdr, gb_operation_msg_size(hdr));
+
+ return 0;
+}
+
+int greybus_alloc_response(struct operation *op, size_t size)
+{
+ size += sizeof(*op->resp);
+ if (size > GB_NETLINK_MTU)
+ return -EINVAL;
+
+ op->resp = malloc(size);
+ if (!op->resp)
+ return -ENOMEM;
+
+ op->resp->operation_id = op->req->operation_id;
+ op->resp->size = size;
+ op->resp->type = op->req->type | 0x80;
+
+ return 0;
+}
+
+static void greybus_free_operation(struct operation *op)
+{
+ free(op->req);
+ free(op->resp);
+ free(op);
+}
+
+int greybus_send_request(uint16_t cport_id, struct operation *op)
+{
+ int len;
+ int ret;
+
+ len = gb_operation_msg_size(op->req);
+ pr_dump(op->req, len);
+
+ op->cport_id = cport_id;
+ TAILQ_INSERT_TAIL(&operations, op, cnode);
+ ret = netlink_send(cport_id, op->req, len);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+static int greybus_send_response(uint16_t cport_id, struct operation *op)
+{
+ int len;
+ int ret;
+
+ len = gb_operation_msg_size(op->resp);
+ pr_dump(op->resp, len);
+
+ ret = netlink_send(cport_id, op->resp, len);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+struct operation *greybus_find_operation(uint16_t cport_id, uint8_t id)
+{
+ struct operation *op;
+ TAILQ_FOREACH(op, &operations, cnode) {
+ if (op->req->operation_id == id) {
+ if (op->cport_id == cport_id) {
+ return op;
+ }
+ }
+ }
+
+ return NULL;
+}
+
+int greybus_handler(uint16_t cport_id, struct gb_operation_msg_hdr *hdr)
+{
+ int ret;
+ struct operation *op;
+
+ pr_dump(hdr, gb_operation_msg_size(hdr));
+
+ if (!gb_handlers[cport_id]) {
+ pr_err("No driver registered for cport %d\n", cport_id);
+ return -EINVAL;
+ }
+
+ if (hdr->type & OP_RESPONSE) {
+ op = greybus_find_operation(cport_id, hdr->operation_id);
+ if (!op) {
+ pr_err("Invalid response id %d on cport %d\n",
+ le16toh(hdr->operation_id), cport_id);
+ return -EINVAL;
+ }
+ TAILQ_REMOVE(&operations, op, cnode);
+ if (_greybus_alloc_response(op, hdr))
+ return -ENOMEM;
+ ret = (gb_handlers[cport_id])(op);
+ } else {
+ op = _greybus_alloc_operation(hdr);
+ if (!op)
+ return -ENOMEM;
+
+ ret = (gb_handlers[cport_id])(op);
+ if (!op->resp) {
+ if (greybus_alloc_response(op, 0)) {
+ pr_err("Failed to alloc greybus response\n");
+ ret = -ENOMEM;
+ goto free_op;
+ }
+ }
+ op->resp->result = greybus_errno_to_result(ret);
+
+ ret = greybus_send_response(cport_id, op);
+ }
+
+ free_op:
+ greybus_free_operation(op);
+
+ return ret;
+}
+
+int greybus_register_driver(uint16_t cport_id, greybus_handler_t handler)
+{
+ if (cport_id >= GB_NETLINK_NUM_CPORT) {
+ pr_err("Invalid cport id %d\n", cport_id);
+ return -EINVAL;
+ }
+ gb_handlers[cport_id] = handler;
+
+ return 0;
+}
+
+int greybus_init(void)
+{
+ TAILQ_INIT(&operations);
+ memset(gb_handlers, 0,
+ sizeof(greybus_handler_t) * GB_NETLINK_NUM_CPORT);
+
+ return greybus_register_driver(0, svc_handler);
+}
diff --git a/main.c b/main.c
index c3ecaf13c7895479ccadf24251cb8b0d7afa2d5e..e9759d302cfe2073477013f12c8d1b4303fd579b 100644
--- a/main.c
+++ b/main.c
@@ -19,6 +19,8 @@
#include <signal.h>
#include <debug.h>
+
+#include "gbridge.h"
#include "netlink.h"
static void signal_handler(int sig)
@@ -34,14 +36,33 @@ int main(int argc, char *argv[])
signal(SIGHUP, signal_handler);
signal(SIGTERM, signal_handler);
+ ret = greybus_init();
+ if (ret) {
+ pr_err("Failed to init Greybus\n");
+ return ret;
+ }
+
ret = netlink_init();
if (ret) {
pr_err("Failed to init netlink\n");
return ret;
}
+ ret = svc_init();
+ if (ret) {
+ pr_err("Failed to init SVC\n");
+ goto err_netlink_exit;
+ }
+
netlink_loop();
netlink_exit();
return 0;
+
+ err_netlink_exit:
+ netlink_cancel();
+ netlink_loop();
+ netlink_exit();
+
+ return ret;
}
diff --git a/netlink.c b/netlink.c
index a1222c0939ad66901f4277d3813b3997318b2c13..782b41c43d6f51fc513114d8209c1f05aab5efb1 100644
--- a/netlink.c
+++ b/netlink.c
@@ -40,6 +40,7 @@ parse_gb_nl_msg(struct nl_cache_ops *unused, struct genl_cmd *cmd,
struct gb_operation_msg_hdr *hdr;
uint16_t hd_cport_id;
size_t len;
+ int ret;
if (!info->attrs[GB_NL_A_DATA] || !info->attrs[GB_NL_A_CPORT])
return -EPROTO;
@@ -54,7 +55,11 @@ parse_gb_nl_msg(struct nl_cache_ops *unused, struct genl_cmd *cmd,
}
if (hd_cport_id == SVC_CPORT) {
- /* TODO: handle SVC operations */
+ ret = greybus_handler(hd_cport_id, hdr);
+ if (ret) {
+ pr_err("Failed to handle svc operation %d: %d\n",
+ hdr->type, ret);
+ }
} else {
/* TODO: transfer data to modules */
}
diff --git a/svc.c b/svc.c
new file mode 100644
index 0000000000000000000000000000000000000000..a5a200b5c3e0be82cecb64002137e5f19fafddd9
--- /dev/null
+++ b/svc.c
@@ -0,0 +1,296 @@
+/*
+ * GBridge (Greybus Bridge)
+ * Copyright (c) 2016 Alexandre Bailon
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <errno.h>
+#include <endian.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <gbridge.h>
+
+/* TODO: Can we use other IDs ? */
+#define ENDO_ID 0x4755
+#define AP_INTF_ID 0x0
+
+static int svc_send_hello_request(void);
+
+static int svc_interface_v_sys_enable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_vsys_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_v_sys_disable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_vsys_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_refclk_enable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_refclk_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_refclk_disable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_refclk_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_unipro_enable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_unipro_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_unipro_disable_response(struct operation *op,
+ uint8_t result_code)
+{
+ struct gb_svc_intf_unipro_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->result_code = result_code;
+
+ return 0;
+}
+
+static int svc_interface_activate_response(struct operation *op,
+ uint8_t intf_type)
+{
+ struct gb_svc_intf_activate_response *resp;
+ size_t op_size = sizeof(*resp);
+
+ if (greybus_alloc_response(op, op_size))
+ return -ENOMEM;
+
+ resp = operation_to_response(op);
+ resp->intf_type = intf_type;
+
+ return 0;
+}
+
+static int svc_ping_request(struct operation *op)
+{
+ return 0;
+}
+
+static int svc_interface_device_id_request(struct operation *op)
+{
+ return 0;
+}
+
+static int svc_interface_v_sys_enable_request(struct operation *op)
+{
+ return svc_interface_v_sys_enable_response(op, GB_SVC_INTF_VSYS_OK);
+}
+
+static int svc_interface_v_sys_disable_request(struct operation *op)
+{
+ return svc_interface_v_sys_disable_response(op, GB_SVC_INTF_VSYS_OK);;
+}
+
+static int svc_interface_refclk_enable_request(struct operation *op)
+{
+ return svc_interface_refclk_enable_response(op, GB_SVC_INTF_REFCLK_OK);
+}
+
+static int svc_interface_refclk_disable_request(struct operation *op)
+{
+ return svc_interface_refclk_disable_response(op, GB_SVC_INTF_REFCLK_OK);
+}
+
+static int svc_interface_unipro_enable_request(struct operation *op)
+{
+ return svc_interface_unipro_enable_response(op, GB_SVC_INTF_UNIPRO_OK);
+}
+
+static int svc_interface_unipro_disable_request(struct operation *op)
+{
+ return svc_interface_unipro_disable_response(op, GB_SVC_INTF_UNIPRO_OK);
+}
+
+static int svc_interface_activate_request(struct operation *op)
+{
+ return svc_interface_activate_response(op, GB_SVC_INTF_TYPE_GREYBUS);
+}
+
+static int svc_interface_hotplug_request(struct operation *op)
+{
+ return -ENOSYS;
+}
+
+static int svc_interface_hot_unplug_request(struct operation *op)
+{
+ return -ENOSYS;
+}
+
+static int svc_handler_response(struct operation *op)
+{
+ switch (op->resp->type & ~OP_RESPONSE) {
+ case GB_REQUEST_TYPE_PROTOCOL_VERSION:
+ return svc_send_hello_request();
+ case GB_SVC_TYPE_SVC_HELLO:
+ case GB_SVC_TYPE_INTF_HOTPLUG:
+ return 0;
+ }
+
+ return -EINVAL;
+}
+
+int svc_handler(struct operation *op)
+{
+ if (op->resp)
+ return svc_handler_response(op);
+
+ switch (op->req->type) {
+ case GB_SVC_TYPE_PING:
+ return svc_ping_request(op);
+ case GB_SVC_TYPE_INTF_DEVICE_ID:
+ return svc_interface_device_id_request(op);
+ case GB_SVC_TYPE_INTF_HOTPLUG:
+ return svc_interface_hotplug_request(op);
+ case GB_SVC_TYPE_INTF_HOT_UNPLUG:
+ return svc_interface_hot_unplug_request(op);
+ case GB_SVC_TYPE_INTF_VSYS_ENABLE:
+ return svc_interface_v_sys_enable_request(op);
+ case GB_SVC_TYPE_INTF_VSYS_DISABLE:
+ return svc_interface_v_sys_disable_request(op);
+ case GB_SVC_TYPE_INTF_REFCLK_ENABLE:
+ return svc_interface_refclk_enable_request(op);
+ case GB_SVC_TYPE_INTF_REFCLK_DISABLE:
+ return svc_interface_refclk_disable_request(op);
+ case GB_SVC_TYPE_INTF_UNIPRO_ENABLE:
+ return svc_interface_unipro_enable_request(op);
+ case GB_SVC_TYPE_INTF_UNIPRO_DISABLE:
+ return svc_interface_unipro_disable_request(op);
+ case GB_SVC_TYPE_INTF_ACTIVATE:
+ return svc_interface_activate_request(op);
+ }
+
+ return -EINVAL;
+}
+
+int svc_send_protocol_version_request(void)
+{
+ struct gb_protocol_version_request req;
+ struct operation *op;
+
+ req.major = GB_SVC_VERSION_MAJOR;
+ req.minor = GB_SVC_VERSION_MINOR;
+
+ op = greybus_alloc_operation(GB_REQUEST_TYPE_PROTOCOL_VERSION,
+ &req, sizeof(req));
+ if (!op)
+ return -ENOMEM;
+
+ return greybus_send_request(0, op);
+}
+
+int svc_send_hello_request(void)
+{
+ struct gb_svc_hello_request req;
+ struct operation *op;
+
+ req.endo_id = htole16(ENDO_ID);
+ req.interface_id = AP_INTF_ID;
+
+ op = greybus_alloc_operation(GB_SVC_TYPE_SVC_HELLO, &req, sizeof(req));
+ if (!op)
+ return -ENOMEM;
+
+ return greybus_send_request(0, op);
+}
+
+int svc_send_intf_hotplug_event(uint8_t intf_id,
+ uint32_t vendor_id,
+ uint32_t product_id, uint64_t serial_number)
+{
+ struct gb_svc_intf_hotplug_request req;
+ struct operation *op;
+
+ req.intf_id = intf_id;
+ req.data.ara_vend_id = htole32(vendor_id);
+ req.data.ara_prod_id = htole32(product_id);
+ req.data.serial_number = htole64(serial_number);
+
+ //FIXME: Use some real version numbers here ?
+ req.data.ddbl1_mfr_id = htole32(1);
+ req.data.ddbl1_prod_id = htole32(1);
+
+ op = greybus_alloc_operation(GB_SVC_TYPE_INTF_HOTPLUG,
+ &req, sizeof(req));
+ if (!op)
+ return -ENOMEM;
+
+ return greybus_send_request(0, op);
+}
+
+int svc_init(void)
+{
+ return svc_send_protocol_version_request();
+}