6 files changed, 99 insertions, 2 deletions

diff --git a/gsm-receiver/src/lib/decoder/cch.c b/gsm-receiver/src/lib/decoder/cch.c
index c6da29d..fa18141 100644
--- a/gsm-receiver/src/lib/decoder/cch.c
+++ b/gsm-receiver/src/lib/decoder/cch.c
@@ -415,10 +415,12 @@ static unsigned char *decode_sacch(GS_CTX *ctx, unsigned char *burst, unsigned i
 		if (FC_check_crc(&fc_ctx, decoded_data, crc_result) == 0)
 		{
 			errors = -1;
-			DEBUGF("error: sacch: parity error (%d fn=%d)\n", errors, ctx->fn);
+			DEBUGF("error: sacch: parity error (%d fn=%d)\n",
+				errors, ctx->fn);
 			return NULL;
 		} else {
-			DEBUGF("Successfully corrected parity bits! (errors=%d fn=%d)\n", errors, ctx->fn);
+			DEBUGF("Successfully corrected parity bits! (errors=%d fn=%d)\n",
+				errors, ctx->fn);
 			memcpy(decoded_data, crc_result, sizeof crc_result);
 			errors = 0;
 		}
@@ -481,3 +483,71 @@ unsigned char *decode_cch(GS_CTX *ctx, unsigned char *e, unsigned int *datalen)
 	   e + 3 * eBLOCK_SIZE, datalen);
 }
 #endif
+
+unsigned char *decode_facch(GS_CTX *ctx, unsigned char *burst, unsigned int *datalen, int offset) {
+
+	int errors, len, data_size;
+	unsigned char conv_data[CONV_SIZE], iBLOCK[BLOCKS * 2][iBLOCK_SIZE],
+	   hl, hn, decoded_data[PARITY_OUTPUT_SIZE];
+	FC_CTX fc_ctx;
+
+	data_size = sizeof ctx->msg;
+	if(datalen)
+		*datalen = 0;
+
+	// unmap the bursts
+	decode_burstmap(iBLOCK[0], burst, &hl, &hn); // XXX ignore stealing bits
+	decode_burstmap(iBLOCK[1], burst + 116, &hl, &hn);
+	decode_burstmap(iBLOCK[2], burst + 116 * 2, &hl, &hn);
+	decode_burstmap(iBLOCK[3], burst + 116 * 3, &hl, &hn);
+	decode_burstmap(iBLOCK[4], burst + 116 * 4, &hl, &hn);
+	decode_burstmap(iBLOCK[5], burst + 116 * 5, &hl, &hn);
+	decode_burstmap(iBLOCK[6], burst + 116 * 6, &hl, &hn);
+	decode_burstmap(iBLOCK[7], burst + 116 * 7, &hl, &hn);
+
+	// remove interleave
+	if (offset == 0)
+		interleave_decode(&ctx->interleave_facch_f1_ctx, conv_data, (unsigned char *)iBLOCK);
+	else
+		interleave_decode(&ctx->interleave_facch_f2_ctx, conv_data, (unsigned char *)iBLOCK);
+	//decode_interleave(conv_data, (unsigned char *)iBLOCK);
+
+	// Viterbi decode
+	errors = conv_decode(decoded_data, conv_data);
+	//DEBUGF("conv_decode: %d\n", errors);
+
+	// check parity
+	// If parity check error detected try to fix it.
+	if (parity_check(decoded_data)) {
+		FC_init(&fc_ctx, 40, 184);
+		unsigned char crc_result[224];
+		if (FC_check_crc(&fc_ctx, decoded_data, crc_result) == 0)
+		{
+			DEBUGF("error: sacch: parity error (errors=%d fn=%d)\n", errors, ctx->fn);
+			errors = -1;
+			return NULL;
+		} else {
+			DEBUGF("Successfully corrected parity bits! (errors=%d fn=%d)\n", errors, ctx->fn);
+			memcpy(decoded_data, crc_result, sizeof crc_result);
+			errors = 0;
+		}
+	}
+
+	if (errors)
+		fprintf(stderr, "WRN: errors=%d fn=%d\n", errors, ctx->fn);
+
+	if ((len = compress_bits(ctx->msg, data_size, decoded_data,
+							DATA_BLOCK_SIZE)) < 0) {
+		fprintf(stderr, "error: compress_bits\n");
+		return NULL;
+	}
+	if (len < data_size) {
+		fprintf(stderr, "error: buf too small (%d < %d)\n",
+		   sizeof(ctx->msg), len);
+		return NULL;
+	}
+
+	if (datalen)
+		*datalen = (unsigned int)len;
+	return ctx->msg;
+}
diff --git a/gsm-receiver/src/lib/decoder/cch.h b/gsm-receiver/src/lib/decoder/cch.h
index a642721..c430714 100644
--- a/gsm-receiver/src/lib/decoder/cch.h
+++ b/gsm-receiver/src/lib/decoder/cch.h
@@ -46,6 +46,7 @@ extern "C" {
 #define eBLOCK_SIZE		(iBLOCK_SIZE + 2)
 
 unsigned char *decode_cch(GS_CTX *ctx, unsigned char *burst, unsigned int *len);
+unsigned char *decode_facch(GS_CTX *ctx, unsigned char *burst, unsigned int *len, int offset);
 //unsigned char *decode_cch(GS_CTX *ctx, unsigned char *, unsigned char *, unsigned char *, unsigned char *, unsigned int *len);
 //unsigned char *decode_cch(GS_CTX *ctx, unsigned char *, unsigned int *);
 
diff --git a/gsm-receiver/src/lib/decoder/gsmstack.c b/gsm-receiver/src/lib/decoder/gsmstack.c
index 6c91395..2bdbf12 100644
--- a/gsm-receiver/src/lib/decoder/gsmstack.c
+++ b/gsm-receiver/src/lib/decoder/gsmstack.c
@@ -96,6 +96,8 @@ GS_new(GS_CTX *ctx)
 
 	memset(ctx, 0, sizeof *ctx);
 	interleave_init(&ctx->interleave_ctx, 456, 114);
+	interleave_init_facch_f(&ctx->interleave_facch_f1_ctx, 456, 114, 0);
+	interleave_init_facch_f(&ctx->interleave_facch_f2_ctx, 456, 114, 4);
 	ctx->fn = -1;
 	ctx->bsic = -1;
 	ctx->gsmtap_fd = -1;
diff --git a/gsm-receiver/src/lib/decoder/gsmstack.h b/gsm-receiver/src/lib/decoder/gsmstack.h
index d02440c..62427d2 100644
--- a/gsm-receiver/src/lib/decoder/gsmstack.h
+++ b/gsm-receiver/src/lib/decoder/gsmstack.h
@@ -23,6 +23,8 @@ typedef struct
 	char msg[23];	/* last decoded message */
 
 	INTERLEAVE_CTX interleave_ctx;
+	INTERLEAVE_CTX interleave_facch_f1_ctx;
+	INTERLEAVE_CTX interleave_facch_f2_ctx;
 
 	struct gs_ts_ctx ts_ctx[8];
 
diff --git a/gsm-receiver/src/lib/decoder/interleave.c b/gsm-receiver/src/lib/decoder/interleave.c
index 7b45927..8d11286 100644
--- a/gsm-receiver/src/lib/decoder/interleave.c
+++ b/gsm-receiver/src/lib/decoder/interleave.c
@@ -25,6 +25,27 @@ interleave_init(INTERLEAVE_CTX *ictx, int size, int block_size)
 }
 
 int
+interleave_init_facch_f(INTERLEAVE_CTX *ictx, int size, int block_size, int block_offset)
+{
+	ictx->trans_size = size;
+	ictx->trans = (unsigned short *)malloc(size * sizeof *ictx->trans);
+
+//	DEBUGF("size: %d\n", size);
+//	DEBUGF("Block size: %d\n", block_size);
+	int j, k, B;
+	for (k = 0; k < size; k++)
+	{
+		B = (k + block_offset) % 8;
+		j = 2 * ((49 * k) % 57) + ((k % 8) / 4);
+		ictx->trans[k] = B * block_size + j;
+		/* Mapping: pos1 goes to pos2: pos1 -> pos2 */
+//		DEBUGF("%d -> %d\n", ictx->trans[k], k);
+	}
+//	exit(0);
+	return 0;
+}
+
+int
 interleave_deinit(INTERLEAVE_CTX *ictx)
 {
 	if (ictx->trans != NULL)
diff --git a/gsm-receiver/src/lib/decoder/interleave.h b/gsm-receiver/src/lib/decoder/interleave.h
index fa1912f..de71a9f 100644
--- a/gsm-receiver/src/lib/decoder/interleave.h
+++ b/gsm-receiver/src/lib/decoder/interleave.h
@@ -13,6 +13,7 @@ typedef struct _interleave_ctx
 } INTERLEAVE_CTX;
 
 int interleave_init(INTERLEAVE_CTX *ictx, int size, int block_size);
+int interleave_init_facch_f(INTERLEAVE_CTX *ictx, int size, int block_size, int block_offset);
 int interleave_deinit(INTERLEAVE_CTX *ictx);
 void interleave_decode(INTERLEAVE_CTX *ictx, unsigned char *dst, unsigned char *src);