cipher

4 files changed, 130 insertions, 7 deletions

diff --git a/include/dectmon.h b/include/dectmon.h
index 8cada3e..802b86b 100644
--- a/include/dectmon.h
+++ b/include/dectmon.h
@@ -36,7 +36,7 @@ enum dect_mm_procedures {
 struct dect_pt {
 	struct list_head			list;
 	struct dect_ie_portable_identity	*portable_identity;
-	struct dect_tbc				*tbc;
+	struct dect_dl				*dl;
 
 	uint8_t					uak[DECT_AUTH_KEY_LEN];
 	uint8_t					dck[DECT_CIPHER_KEY_LEN];
@@ -89,6 +89,8 @@ struct dect_tbc {
 	struct dect_mbc				mbc[2];
 	struct dect_dl				dl;
 	bool					ciphered;
+
+	uint8_t					ks[2 * 45];
 };
 
 extern void dect_mac_rcv(struct dect_msg_buf *mb, uint8_t slot);
diff --git a/src/dlc.c b/src/dlc.c
index 9340519..99741c1 100644
--- a/src/dlc.c
+++ b/src/dlc.c
@@ -9,6 +9,7 @@
  */
 
 #include <assert.h>
+#include <stdlib.h>
 #include <stdio.h>
 #include <dect/libdect.h>
 #include <dectmon.h>
diff --git a/src/mac.c b/src/mac.c
index 65d66e3..9e9b072 100644
--- a/src/mac.c
+++ b/src/mac.c
@@ -552,6 +552,29 @@ static struct dect_tbc *dect_tbc_init(uint32_t pmid)
 	return tbc;
 }
 
+static void dect_dsc_cipher(struct dect_tbc *tbc, struct dect_msg_buf *mb)
+{
+	unsigned int i;
+	uint8_t *ks;
+
+	if (mb->slot < 12)
+		ks = tbc->ks;
+	else
+		ks = tbc->ks + 45;
+
+	switch (mb->data[0] & DECT_HDR_TA_MASK) {
+	case DECT_TI_CT_PKT_0:
+	case DECT_TI_CT_PKT_1:
+		for (i = 0; i < 5; i++)
+			mb->data[i + 1] ^= ks[i];
+	default:
+		break;
+	}
+
+	for (i = 0; i < DECT_B_FIELD_SIZE; i++)
+		mb->data[i + 8] ^= ks[i + 5];
+}
+
 static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot,
 			 struct dect_msg_buf *mb, struct dect_tail_msg *tm)
 {
@@ -560,6 +583,14 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot,
 	unsigned int i;
 	bool cf;
 
+	if (tbc->ciphered) {
+		if (slot < 12)
+			dect_dsc_keystream(dect_dsc_iv(mb->mfn, mb->frame),
+					   tbc->dl.pt->dck,
+					   tbc->ks, sizeof(tbc->ks));
+		dect_dsc_cipher(tbc, mb);
+	}
+
 	mbc = &tbc->mbc[slot < 12 ? DECT_MODE_FP : DECT_MODE_PP];
 	b_id = (mb->data[0] & DECT_HDR_BA_MASK);
 
@@ -607,13 +638,32 @@ static void dect_tbc_rcv(struct dect_tbc *tbc, uint8_t slot,
 		break;
 	}
 
-	if (tm->type == DECT_TM_TYPE_BCCTRL ||
-	    tm->type == DECT_TM_TYPE_ACCTRL) {
+	switch (tm->type) {
+	case DECT_TM_TYPE_BCCTRL:
+	case DECT_TM_TYPE_ACCTRL:
 		if (tm->cctl.cmd == DECT_CCTRL_RELEASE) {
 			slots[slot].tbc		       = NULL;
 			slots[dect_tdd_slot(slot)].tbc = NULL;
 			free(tbc);
 		}
+		break;
+	case DECT_TM_TYPE_ENCCTRL:
+		switch (tm->encctl.cmd) {
+		case DECT_ENCCTRL_START_REQUEST:
+			printf("\n");
+			break;
+		case DECT_ENCCTRL_START_CONFIRM:
+			printf("ciphering enabled: %s\n", slot < 12 ? "FP->PP" : "PP->FP");
+			break;
+		case DECT_ENCCTRL_START_GRANT:
+			printf("ciphering enabled: %s\n", slot < 12 ? "FP->PP" : "PP->FP");
+			break;
+		default:
+			break;
+		}
+		break;
+	default:
+		break;
 	}
 }
 
diff --git a/src/nwk.c b/src/nwk.c
index 48395b3..7e78bf5 100644
--- a/src/nwk.c
+++ b/src/nwk.c
@@ -8,7 +8,9 @@
  * published by the Free Software Foundation.
  */
 
+#include <stdlib.h>
 #include <stdio.h>
+#include <limits.h>
 
 #include <dect/libdect.h>
 #include <dect/s_fmt.h>
@@ -72,6 +74,66 @@ static const char * const nwk_msg_types[256] = {
 		ie = NULL;			\
 	} while (0)
 
+static FILE *dect_keyfile_open(const char *mode)
+{
+	char name[PATH_MAX];
+
+	snprintf(name, sizeof(name), "%s/%s", getenv("HOME"), "dectmon.keys");
+	return fopen(name, mode);
+}
+
+static void dect_pt_write_uak(const struct dect_pt *pt)
+{
+	char ipei[DECT_IPEI_STRING_LEN];
+	unsigned int i;
+	FILE *f;
+
+	f = dect_keyfile_open("w");
+	if (f == NULL)
+		return;
+	dect_format_ipei_string(&pt->portable_identity->ipui.pun.n.ipei, ipei);
+
+	fprintf(f, "%s|", ipei);
+	for (i = 0; i < DECT_AUTH_KEY_LEN; i++)
+		fprintf(f, "%02x", pt->uak[i]);
+	fprintf(f, "\n");
+
+	fclose(f);
+}
+
+static void dect_pt_read_uak(struct dect_pt *pt)
+{
+	char ipei[DECT_IPEI_STRING_LEN];
+	uint8_t uak[DECT_AUTH_KEY_LEN];
+	struct dect_ipui ipui;
+	unsigned int i;
+	FILE *f;
+
+	f = dect_keyfile_open("r");
+	if (f == NULL)
+		return;
+
+	if (fscanf(f, "%13s|", ipei) != 1)
+		goto err;
+
+	for (i = 0; i < DECT_AUTH_KEY_LEN; i++) {
+		if (fscanf(f, "%02hhx", &uak[i]) != 1)
+			goto err;
+	}
+
+	memset(&ipui, 0, sizeof(ipui));
+	ipui.put = DECT_IPUI_N;
+	if (!dect_parse_ipei_string(&ipui.pun.n.ipei, ipei))
+		goto err;
+
+	if (dect_ipui_cmp(&ipui, &pt->portable_identity->ipui))
+		goto err;
+
+	memcpy(pt->uak, uak, sizeof(pt->uak));
+err:
+	fclose(f);
+}
+
 static struct dect_pt *dect_pt_lookup(struct dect_ie_portable_identity *portable_identity)
 {
 	struct dect_pt *pt;
@@ -95,6 +157,9 @@ static struct dect_pt *dect_pt_init(struct dect_ie_portable_identity *portable_i
 	pt->portable_identity = dect_ie_hold(portable_identity);
 	list_add_tail(&pt->list, &dect_pt_list);
 
+	dect_pt_read_uak(pt);
+	printf("new PT\n");
+
 	return pt;
 }
 
@@ -123,7 +188,8 @@ static void dect_pt_track_key_allocation(struct dect_pt *pt, uint8_t msgtype,
 		return;
 	case DECT_MM_AUTHENTICATION_REQUEST:
 		if (pt->procedure != DECT_MM_KEY_ALLOCATION ||
-		    pt->last_msg != DECT_MM_KEY_ALLOCATE)
+		    (pt->last_msg != DECT_MM_KEY_ALLOCATE &&
+		     pt->last_msg != DECT_MM_AUTHENTICATION_REQUEST))
 			return;
 
 		if (ie->id == DECT_IE_RES)
@@ -159,11 +225,12 @@ static void dect_pt_track_key_allocation(struct dect_pt *pt, uint8_t msgtype,
 
 		dect_hexdump("DCK", dck, sizeof(dck));
 		memcpy(pt->dck, dck, sizeof(pt->dck));
+
+		dect_pt_write_uak(pt);
 	} else
 		printf("authentication failed\n");
 
 release:
-	dect_ie_release(dh, pt->portable_identity);
 	dect_ie_release(dh, pt->rs);
 	dect_ie_release(dh, pt->rand_f);
 	dect_ie_release(dh, pt->res);
@@ -221,8 +288,10 @@ static void dect_pt_track_auth(struct dect_pt *pt, uint8_t msgtype,
 
 	if (res1.value == pt->res->value) {
 		printf("authentication successful\n");
-		if (pt->auth_type->flags & DECT_AUTH_FLAG_UPC)
+		if (pt->auth_type->flags & DECT_AUTH_FLAG_UPC) {
+			dect_hexdump("DCK", dck, sizeof(dck));
 			memcpy(pt->dck, dck, sizeof(pt->dck));
+		}
 	} else
 		printf("authentication failed\n");
 
@@ -242,7 +311,7 @@ static void dect_pt_track_ciphering(struct dect_pt *pt, uint8_t msgtype,
 	case DECT_MM_CIPHER_REQUEST:
 		if (pt->procedure != DECT_MM_NONE)
 			return;
-		pt->tbc->ciphered = true;
+		pt->dl->tbc->ciphered = true;
 		break;
 	default:
 		return;
@@ -277,6 +346,7 @@ void dect_dl_data_ind(struct dect_dl *dl, struct dect_msg_buf *mb)
 			if (pt == NULL)
 				pt = dect_pt_init((void *)common);
 			dl->pt = pt;
+			pt->dl = dl;
 		}
 
 		if (dl->pt != NULL) {