[PATCH 10/25] Process md_create_mdl_req command in state connected

[Santiago Carot-Nemesio <sancane@gmail.com>]

From: Santiago Carot Nemesio <sancane@gmail.com>

---
 mcap/mcap.c |  153 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 151 insertions(+), 2 deletions(-)

diff --git a/mcap/mcap.c b/mcap/mcap.c
index 3a4b2b9..1bc3ed5 100644
--- a/mcap/mcap.c
+++ b/mcap/mcap.c
@@ -95,6 +95,40 @@ static int send4B_cmd(struct mcap_mcl *mcl, uint8_t oc, uint8_t rc,
 	return sent;
 }
 
+static int send5B_cmd(struct mcap_mcl *mcl, uint8_t oc, uint8_t rc,
+						uint16_t mdl, uint8_t param)
+{
+	uint8_t *rsp;
+	mcap5B_rsp *suc;
+	int sent;
+
+	rsp = g_malloc0(sizeof(mcap5B_rsp));
+
+	suc = (mcap5B_rsp *)rsp;
+	suc->op = oc;
+	suc->rc = rc;
+	suc->mdl = htons(mdl);
+	suc->param = param;
+
+	sent = mcap_send_data(g_io_channel_unix_get_fd(mcl->cc), rsp,
+							sizeof(mcap5B_rsp));
+	g_free(rsp);
+	return sent;
+}
+
+static gint compare_mdl(gconstpointer a, gconstpointer b)
+{
+	const struct mcap_mdl *mdla = a;
+	const struct mcap_mdl *mdlb = b;
+
+	if (mdla->mdlid == mdlb->mdlid)
+		return 0;
+	else if (mdla->mdlid < mdlb->mdlid)
+		return -1;
+	else
+		return 1;
+}
+
 static void default_mdl_connected_cb(struct mcap_mdl *mdl, gpointer data)
 {
 	debug("MCAP Unmanaged mdl connection");
@@ -425,11 +459,126 @@ bdaddr_t mcap_mcl_get_addr(struct mcap_mcl *mcl)
 	return mcl->addr;
 }
 
-/* Function used to process commands depending of MCL state */
+static void error_cmd_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	uint16_t mdlr;
+
+	if (cmd[0] <= MCAP_MD_DELETE_MDL_RSP) {
+		/* Standard Op Code request is invalid in current state */
+		error("Invalid cmd received (op code = %d) in state %d",
+							cmd[0],	mcl->state);
+		/* Get mdlid sended to generate appropiate response if it is possible */
+		mdlr = len < sizeof(mcap_md_req) ? MCAP_MDLID_RESERVED :
+						ntohs(((mcap_md_req *)cmd)->mdl);
+		send4B_cmd(mcl, cmd[0]+1, MCAP_INVALID_OPERATION, mdlr);
+	} else {
+		error("Unknown cmd request received (op code = %d) in state %d",
+							cmd[0], mcl->state);
+		send4B_cmd(mcl, MCAP_ERROR_RSP, MCAP_INVALID_OP_CODE,
+							MCAP_MDLID_RESERVED);
+	}
+}
+
+static struct mcap_mdl *get_mdl(struct mcap_mcl *mcl, uint16_t mdlid)
+{
+	GSList *l;
+	struct mcap_mdl *mdl;
+
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+		if (mdlid == mdl->mdlid)
+			return mdl;
+	}
+
+	return NULL;
+}
+
+static void process_md_create_mdl_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_create_mdl_req *req;
+	struct mcap_mdl *mdl;
+	uint16_t mdl_id;
+	uint8_t mdep_id;
+	uint8_t cfga, conf;
+	uint8_t rsp;
+
+	if (len != sizeof(mcap_md_create_mdl_req)) {
+		send4B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP,
+				MCAP_INVALID_PARAM_VALUE, MCAP_MDLID_RESERVED);
+		return;
+	}
+
+	req = (mcap_md_create_mdl_req *)cmd;
+
+	mdl_id = ntohs(req->mdl);
+	if ((mdl_id < MCAP_MDLID_INITIAL) || (mdl_id > MCAP_MDLID_FINAL)) {
+		send4B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDL, mdl_id);
+		return;
+	}
+
+	mdep_id = req->mdep;
+	if (mdep_id > MCAP_MDEPID_FINAL) {
+		send4B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDEP, mdl_id);
+		return;
+	}
+
+	cfga = conf = req->conf;
+	/* Callback to upper layer */
+	rsp = mcl->cb->mdl_conn_req(mcl, mdep_id, mdl_id, &conf,
+							mcl->cb->user_data);
+	if (mcl->state == MCL_IDLE) {
+		/* MCL has been closed int the callback */
+		return;
+	}
+
+	if ((cfga != 0) && (cfga != conf)) {
+		/* Remote device set default configuration but upper profile */
+		/* has changed it. Protocol Error: force closing the MCL by */
+		/* remote device using UNESPECIFIED_ERROR response */
+		send4B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_UNESPECIFIED_ERROR,
+								mdl_id);
+		return;
+	}
+	if (rsp != MCAP_SUCCESS) {
+		send4B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, rsp, mdl_id);
+		return;
+	}
 
+	mdl = get_mdl(mcl, mdl_id);
+	if (!mdl) {
+		mdl = g_malloc0(sizeof(struct mcap_mdl));
+		mdl->mcl = mcl;
+		mdl->mdlid = mdl_id;
+	} else if (mdl->state == MDL_CONNECTED) {
+		/* MCAP specification says that we should close the MCL if
+		 * it is open when we receive a MD_CREATE_MDL_REQ */
+		shutdown_mdl(mdl);
+	}
+	mdl->mdep_id = mdep_id;
+	mdl->state = MDL_WAITING;
+	mcl->mdls = g_slist_insert_sorted(mcl->mdls, mdl, compare_mdl);
+
+	mcl->state = MCL_PENDING;
+	send5B_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_SUCCESS, mdl_id, conf);
+}
+
+
+/* Function used to process commands depending of MCL state */
 static void proc_req_connected(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
 {
-	/* TODO: Proccessing commands in CONNECTED state */
+	switch (cmd[0]) {
+	case MCAP_MD_CREATE_MDL_REQ:
+		process_md_create_mdl_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_RECONNECT_MDL_REQ:
+		/*process_md_reconnect_mdl_req(mcl, cmd, len);*/
+		break;
+	case MCAP_MD_DELETE_MDL_REQ:
+		/*process_md_delete_mdl_req(mcl, cmd, len);*/
+		break;
+	default:
+		error_cmd_rsp(mcl, cmd, len);
+	}
 }
 
 static void proc_req_pending(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
-- 
1.6.3.3