lce: maintain LCE location table

Maintain LCE location table and use it for determining paging mode
and setup capabilities.

Signed-off-by: Patrick McHardy <kaber@trash.net>

4 files changed, 131 insertions, 21 deletions

diff --git a/src/clms.c b/src/clms.c
index 3cbae54..d2ba857 100644
--- a/src/clms.c
+++ b/src/clms.c
@@ -173,7 +173,7 @@ static void dect_clms_send_fixed(struct dect_handle *dh,
 		section++;
 	}
 deliver:
-	dect_lce_broadcast(dh, mb, true);
+	dect_lce_broadcast(dh, mb, true, false);
 }
 
 /**
diff --git a/src/lce.c b/src/lce.c
index 34eafed..a7c30f8 100644
--- a/src/lce.c
+++ b/src/lce.c
@@ -204,7 +204,7 @@ static ssize_t dect_mbuf_send(const struct dect_handle *dh,
 	msg->msg_namelen	= 0;
 	msg->msg_iov		= &iov;
 	msg->msg_iovlen		= 1;
-	msg->msg_flags		= MSG_NOSIGNAL;
+	msg->msg_flags		|= MSG_NOSIGNAL;
 
 	iov.iov_base		= mb->data;
 	iov.iov_len		= mb->len;
@@ -216,24 +216,31 @@ static ssize_t dect_mbuf_send(const struct dect_handle *dh,
 	return len;
 }
 
-#if 0
 /*
  * Location Table
  */
 
+#define dect_ie_update(pos, ie)			\
+	do {					\
+		if (ie == NULL)			\
+			break;			\
+		dect_ie_put(dh, pos);		\
+		pos = dect_ie_hold(ie);		\
+	} while (0)
+
 static struct dect_lte *dect_lte_get_by_ipui(const struct dect_handle *dh,
 					     const struct dect_ipui *ipui)
 {
 	struct dect_lte *lte;
 
-	list_for_each_entry(lte, &dh->ldb.entries, list) {
+	list_for_each_entry(lte, &dh->ldb, list) {
 		if (!dect_ipui_cmp(&lte->ipui, ipui))
 			return lte;
 	}
 	return NULL;
 }
 
-static struct dect_lte *dect_lte_alloc(const struct dect_handle *dh,
+static struct dect_lte *dect_lte_alloc(struct dect_handle *dh,
 				       const struct dect_ipui *ipui)
 {
 	struct dect_lte *lte;
@@ -241,10 +248,74 @@ static struct dect_lte *dect_lte_alloc(const struct dect_handle *dh,
 	lte = dect_malloc(dh, sizeof(*lte));
 	if (lte == NULL)
 		return NULL;
-	memcpy(&lte->ipui, ipui, sizeof(lte->ipui));
+	lte->ipui = *ipui;
+
+	list_add_tail(&lte->list, &dh->ldb);
 	return lte;
 }
-#endif
+
+static void dect_lte_release(struct dect_handle *dh, struct dect_lte *lte)
+{
+	dect_ie_put(dh, lte->setup_capability);
+	dect_ie_put(dh, lte->terminal_capability);
+	list_del(&lte->list);
+	dect_free(dh, lte);
+}
+
+void dect_lte_update(struct dect_handle *dh, const struct dect_ipui *ipui,
+		     struct dect_ie_setup_capability *setup_capability,
+		     struct dect_ie_terminal_capability *terminal_capability)
+{
+	struct dect_lte *lte;
+
+	lte = dect_lte_get_by_ipui(dh, ipui);
+	if (lte == NULL) {
+		lte = dect_lte_alloc(dh, ipui);
+		if (lte == NULL)
+			return;
+	}
+
+	dect_ie_update(lte->setup_capability, setup_capability);
+	dect_ie_update(lte->terminal_capability, terminal_capability);
+}
+
+static enum dect_setup_capabilities
+dect_setup_capability(const struct dect_handle *dh,
+		      const struct dect_ipui *ipui)
+{
+	const struct dect_lte *lte;
+
+	lte = dect_lte_get_by_ipui(dh, ipui);
+	if (lte == NULL ||
+	    lte->setup_capability == NULL)
+		return DECT_SETUP_NO_FAST_SETUP;
+	return lte->setup_capability->setup_capability;
+}
+
+static enum dect_page_capabilities
+dect_page_capability(const struct dect_handle *dh,
+		     const struct dect_ipui *ipui)
+{
+	const struct dect_lte *lte;
+
+	lte = dect_lte_get_by_ipui(dh, ipui);
+	if (lte == NULL ||
+	    lte->setup_capability == NULL)
+		return DECT_PAGE_CAPABILITY_NORMAL_PAGING;
+	return lte->setup_capability->page_capability;
+}
+
+static uint64_t dect_profile_indicator(const struct dect_handle *dh,
+				       const struct dect_ipui *ipui)
+{
+	const struct dect_lte *lte;
+
+	lte = dect_lte_get_by_ipui(dh, ipui);
+	if (lte == NULL ||
+	    lte->terminal_capability == NULL)
+		return 0;
+	return lte->terminal_capability->profile_indicator;
+}
 
 /*
  * Data links
@@ -822,7 +893,8 @@ static struct dect_data_link *dect_ddl_establish(struct dect_handle *dh,
 	ddl->state = DECT_DATA_LINK_ESTABLISH_PENDING;
 	dect_ddl_set_ipui(dh, ddl, ipui);
 
-	if (dh->mode == DECT_MODE_FP) {
+	if (dh->mode == DECT_MODE_FP ||
+	    dect_setup_capability(dh, ipui) != DECT_SETUP_NO_FAST_SETUP) {
 		ddl->page_timer = dect_timer_alloc(dh);
 		if (ddl->page_timer == NULL)
 			goto err2;
@@ -951,7 +1023,7 @@ err1:
 
 ssize_t dect_lce_broadcast(const struct dect_handle *dh,
 			   const struct dect_msg_buf *mb,
-			   bool long_page)
+			   bool long_page, bool fast_page)
 {
 	struct msghdr msg;
 	struct dect_bsap_auxdata aux;
@@ -966,6 +1038,7 @@ ssize_t dect_lce_broadcast(const struct dect_handle *dh,
 		memset(cmsg_buf.buf, 0, sizeof(cmsg_buf.buf));
 		msg.msg_control		= &cmsg_buf;
 		msg.msg_controllen	= sizeof(cmsg_buf);
+		msg.msg_flags		= 0;
 
 		cmsg			= CMSG_FIRSTHDR(&msg);
 		cmsg->cmsg_len		= CMSG_LEN(sizeof(aux));
@@ -977,6 +1050,8 @@ ssize_t dect_lce_broadcast(const struct dect_handle *dh,
 	} else {
 		msg.msg_control		= NULL;
 		msg.msg_controllen	= 0;
+		if (fast_page)
+			msg.msg_flags	= MSG_OOB;
 	}
 
 	dect_mbuf_dump(DECT_DEBUG_LCE, mb, "LCE: BCAST TX");
@@ -1009,7 +1084,7 @@ int dect_lce_group_ring_req(struct dect_handle *dh,
 	page |= DECT_TPUI_CBI & DECT_LCE_SHORT_PAGE_TPUI_MASK;
 	msg->information = __cpu_to_be16(page);
 
-	return dect_lce_broadcast(dh, mb, false);
+	return dect_lce_broadcast(dh, mb, false, false);
 }
 EXPORT_SYMBOL(dect_lce_group_ring_req);
 
@@ -1019,6 +1094,7 @@ static int dect_lce_send_short_page(const struct dect_handle *dh,
 	DECT_DEFINE_MSG_BUF_ONSTACK(_mb), *mb = &_mb;
 	struct dect_short_page_msg *msg;
 	struct dect_tpui tpui;
+	bool fast_page = false;
 	uint16_t page;
 
 	msg = dect_mbuf_put(mb, sizeof(*msg));
@@ -1028,7 +1104,11 @@ static int dect_lce_send_short_page(const struct dect_handle *dh,
 	       DECT_LCE_SHORT_PAGE_TPUI_MASK;
 	msg->information = __cpu_to_be16(page);
 
-	return dect_lce_broadcast(dh, mb, false);
+	if (dect_page_capability(dh, ipui) ==
+	    DECT_PAGE_CAPABILITY_FAST_AND_NORMAL_PAGING)
+		fast_page = true;
+
+	return dect_lce_broadcast(dh, mb, false, fast_page);
 }
 
 static int dect_lce_send_full_page(const struct dect_handle *dh,
@@ -1038,6 +1118,7 @@ static int dect_lce_send_full_page(const struct dect_handle *dh,
 	struct dect_full_page_msg *msg;
 	struct dect_tpui tpui;
 	uint8_t ipui_buf[8];
+	bool fast_page = false;
 	uint32_t page;
 
 	msg = dect_mbuf_put(mb, sizeof(*msg));
@@ -1062,16 +1143,20 @@ static int dect_lce_send_full_page(const struct dect_handle *dh,
 	}
 	msg->information = __cpu_to_be32(page);
 
-	return dect_lce_broadcast(dh, mb, false);
+	if (dect_page_capability(dh, ipui) ==
+	    DECT_PAGE_CAPABILITY_FAST_AND_NORMAL_PAGING)
+		fast_page = true;
+
+	return dect_lce_broadcast(dh, mb, false, fast_page);
 }
 
 static int dect_lce_page(const struct dect_handle *dh,
 			 const struct dect_ipui *ipui)
 {
-	if (1)
-		return dect_lce_send_short_page(dh, ipui);
-	else
+	if (dect_profile_indicator(dh, ipui) & DECT_PROFILE_NG_DECT_PART_1)
 		return dect_lce_send_full_page(dh, ipui);
+	else
+		return dect_lce_send_short_page(dh, ipui);
 }
 
 static void dect_ddl_page_timer(struct dect_handle *dh, struct dect_timer *timer)
@@ -1666,11 +1751,15 @@ err1:
 
 void dect_lce_exit(struct dect_handle *dh)
 {
-	struct dect_data_link *ddl, *next;
+	struct dect_data_link *ddl, *ddl_next;
+	struct dect_lte *lte, *lte_next;
 
-	list_for_each_entry_safe(ddl, next, &dh->links, list)
+	list_for_each_entry_safe(ddl, ddl_next, &dh->links, list)
 		dect_ddl_shutdown(dh, ddl);
 
+	list_for_each_entry_safe(lte, lte_next, &dh->ldb, list)
+		dect_lte_release(dh, lte);
+
 	if (dh->mode == DECT_MODE_FP) {
 		dect_fd_unregister(dh, dh->s_sap);
 		dect_close(dh, dh->s_sap);
diff --git a/src/libdect.c b/src/libdect.c
index 8b77f38..2038943 100644
--- a/src/libdect.c
+++ b/src/libdect.c
@@ -37,6 +37,7 @@ static struct dect_handle *dect_alloc_handle(struct dect_ops *ops)
 	memset(dh, 0, sizeof(*dh) + ops->priv_size);
 
 	dh->ops = ops;
+	init_list_head(&dh->ldb);
 	init_list_head(&dh->links);
 	init_list_head(&dh->mme_list);
 	return dh;
diff --git a/src/mm.c b/src/mm.c
index dd70322..eebab98 100644
--- a/src/mm.c
+++ b/src/mm.c
@@ -1448,15 +1448,21 @@ int dect_mm_access_rights_res(struct dect_handle *dh,
 			      const struct dect_mm_access_rights_param *param)
 {
 	struct dect_mm_procedure *mp = &mme->procedure[DECT_TRANSACTION_RESPONDER];
+	struct dect_mm_access_rights_param *req;
 	int err;
 
 	mm_debug_entry(mme, "MM_ACCESS_RIGHTS-res: accept: %u", accept);
 	if (mp->type != DECT_MMP_ACCESS_RIGHTS)
 		return -1;
 
-	if (accept)
+	if (accept) {
+		req = (struct dect_mm_access_rights_param *)mp->iec;
+		dect_lte_update(dh, &req->portable_identity->ipui,
+				req->setup_capability,
+				req->terminal_capability);
+
 		err = dect_mm_send_access_rights_accept(dh, mme, param);
-	else
+	} else
 		err = dect_mm_send_access_rights_reject(dh, mme, param);
 
 	if (err < 0)
@@ -1473,11 +1479,13 @@ static void dect_mm_rcv_access_rights_request(struct dect_handle *dh,
 {
 	struct dect_mm_access_rights_request_msg msg;
 	struct dect_mm_access_rights_param *param;
+	struct dect_mm_procedure *mp;
 	enum dect_sfmt_error err;
 
 	mm_debug(mme, "ACCESS-RIGHTS-REQUEST");
 	if (dect_mm_procedure_respond(dh, mme, DECT_MMP_ACCESS_RIGHTS) < 0)
 		return;
+	mp = mme->current;
 
 	err = dect_parse_sfmt_msg(dh, &mm_access_rights_request_msg_desc,
 				  &msg.common, mb);
@@ -1503,6 +1511,8 @@ static void dect_mm_rcv_access_rights_request(struct dect_handle *dh,
 	param->escape_to_proprietary	= dect_ie_hold(msg.escape_to_proprietary);
 	param->codec_list		= dect_ie_hold(msg.codec_list);
 
+	mp->iec = dect_ie_collection_hold(param);
+
 	mm_debug(mme, "MM_ACCESS_RIGHTS-ind");
 	dh->ops->mm_ops->mm_access_rights_ind(dh, mme, param);
 	dect_ie_collection_put(dh, param);
@@ -1981,15 +1991,21 @@ int dect_mm_locate_res(struct dect_handle *dh, struct dect_mm_endpoint *mme,
 		       bool accept, const struct dect_mm_locate_param *param)
 {
 	struct dect_mm_procedure *mp = &mme->procedure[DECT_TRANSACTION_RESPONDER];
+	struct dect_mm_locate_param *req;
 	int err;
 
 	mm_debug_entry(mme, "MM_LOCATE-res: accept: %u", accept);
 	if (mp->type != DECT_MMP_LOCATION_REGISTRATION)
 		return -1;
 
-	if (accept)
+	if (accept) {
+		req = (struct dect_mm_locate_param *)mp->iec;
+		dect_lte_update(dh, &req->portable_identity->ipui,
+				req->setup_capability,
+				req->terminal_capability);
+
 		err = dect_mm_send_locate_accept(dh, mme, param);
-	else
+	} else
 		err = dect_mm_send_locate_reject(dh, mme, param);
 
 	if (err < 0)
@@ -2017,11 +2033,13 @@ static void dect_mm_rcv_locate_request(struct dect_handle *dh,
 {
 	struct dect_mm_locate_request_msg msg;
 	struct dect_mm_locate_param *param;
+	struct dect_mm_procedure *mp;
 	enum dect_sfmt_error err;
 
 	mm_debug(mme, "LOCATE-REQUEST");
 	if (dect_mm_procedure_respond(dh, mme, DECT_MMP_LOCATION_REGISTRATION) < 0)
 		return;
+	mp = mme->current;
 
 	err = dect_parse_sfmt_msg(dh, &mm_locate_request_msg_desc,
 				  &msg.common, mb);
@@ -2051,6 +2069,8 @@ static void dect_mm_rcv_locate_request(struct dect_handle *dh,
 	param->escape_to_proprietary	= dect_ie_hold(msg.escape_to_proprietary);
 	param->codec_list		= dect_ie_hold(msg.codec_list);
 
+	mp->iec = dect_ie_collection_hold(param);
+
 	mm_debug(mme, "MM_LOCATE-ind");
 	dh->ops->mm_ops->mm_locate_ind(dh, mme, param);
 	dect_ie_collection_put(dh, param);