commit 7a49f811b1ed121c35c05673abdbd6ea64ed2c06
Author: Christian Vogel <vogelchr@vogel.cx>
Date:   Tue Apr 13 21:34:42 2010 +0200

    Add CRC16 to HDLC frames.

diff --git a/src/target/firmware/comm/sercomm.c b/src/target/firmware/comm/sercomm.c
index b6bda50..b952a9c 100644
--- a/src/target/firmware/comm/sercomm.c
+++ b/src/target/firmware/comm/sercomm.c
@@ -25,6 +25,7 @@
 #include <errno.h>
 
 #include <osmocore/msgb.h>
+#include <osmocore/crc16.h>
 
 #ifdef HOST_BUILD
 #define SERCOMM_RX_MSG_SIZE	2048
@@ -63,6 +64,7 @@ static struct {
 		struct llist_head dlci_queues[_SC_DLCI_MAX];
 		struct msgb *msg;
 		enum rx_state state;
+		uint16_t crc16_dlci_mask;  /* set 1<<n to enable CRC for dlci n */
 		uint8_t *next_char;
 	} tx;
 
@@ -71,12 +73,20 @@ static struct {
 		dlci_cb_t dlci_handler[_SC_DLCI_MAX];
 		struct msgb *msg;
 		enum rx_state state;
+		uint32_t rxctr,badcrc; /* rcv and bad crc counter */
 		uint8_t dlci;
 		uint8_t ctrl;
 	} rx;
 	
 } sercomm;
 
+void
+sercomm_get_rx_ctr(unsigned int *rxctr,unsigned int *badcrc){
+	*rxctr = sercomm.rx.rxctr;
+	*badcrc = sercomm.rx.badcrc;
+};
+
+
 void sercomm_init(void)
 {
 	unsigned int i;
@@ -86,6 +96,10 @@ void sercomm_init(void)
 	sercomm.rx.msg = NULL;
 	sercomm.initialized = 1;
 
+	/* Enable CRC on CONSOLE and ECHO DLCI only for now. Note that as
+	   a special case bad CRC frames are NOT discarded on ECHO! */
+	sercomm.tx.crc16_dlci_mask = (1<<SC_DLCI_CONSOLE)|(1<<SC_DLCI_ECHO);
+
 #ifndef HOST_BUILD
        /* Enable sending back ECHO only on the target! */
        sercomm_register_rx_cb(SC_DLCI_ECHO,&sercomm_sendmsg);
@@ -108,6 +122,13 @@ void sercomm_sendmsg(uint8_t dlci, struct msgb *msg)
 	hdr[0] = dlci;
 	hdr[1] = HDLC_C_UI;
 
+	/* check if we should append CRC16 for this DLCI? */
+	if(sercomm.tx.crc16_dlci_mask & (1<<dlci)){
+		uint16_t crc16_data = osmocore_crc16(0,msg->data+2,msg->len-2);
+		msgb_put_u16(msg,crc16_data); // put MSB, then LSB
+		hdr[1] |= HDLC_C_CRC16_BIT;
+	}
+
 	/* This functiion can be called from any context: FIQ, IRQ
 	 * and supervisor context.  Proper locking is important! */
 	local_irq_save(flags);
@@ -212,6 +233,42 @@ static void dispatch_rx_msg(uint8_t dlci, struct msgb *msg)
 	sercomm.rx.dlci_handler[dlci](dlci, msg);
 }
 
+/* check if crc16 of message msg is valid: If it is, strip
+   the two crc16 bytes and return 0, if it's invalid return 1 */
+static int sercomm_chk_strip_crc16(struct msgb *msg)
+{
+	uint16_t crc16_msg,crc16_data;
+	if(msg->len < 2) // too short to contain a crc16
+		return 1;
+
+	msg->len -= 2;  /* this does the opposite of msgb_put(msg,2) */
+	msg->tail -= 2; /* to trim the crc in the last two bytes */
+
+	crc16_data = osmocore_crc16(0,msg->data,msg->len);
+	crc16_msg = (msg->data[msg->len]<<8) + msg->data[msg->len+1];
+
+#ifdef HOST_BUILD
+	if(crc16_msg != crc16_data){
+		fprintf(stderr,"\r\033[0;31mBAD CRC\033[0m");
+		fprintf(stderr," on dlci %d: recv %04x calc %04x\n",
+			sercomm.rx.dlci,crc16_msg,crc16_data);
+	}
+#endif
+
+	if(crc16_msg != crc16_data) // crc error
+		return 1;
+
+	return 0;
+}
+
+void
+sercomm_tx_set_crc16(int dlci,int onoff){
+	if(onoff)
+		sercomm.tx.crc16_dlci_mask |= (1<<dlci);
+	else
+		sercomm.tx.crc16_dlci_mask &= ~(1<<dlci);
+}
+
 /* the driver has received one byte, pass it into sercomm layer */
 int sercomm_drv_rx_char(uint8_t ch)
 {
@@ -251,8 +308,22 @@ int sercomm_drv_rx_char(uint8_t ch)
 			sercomm.rx.state = RX_ST_ESCAPE;
 			break;
 		} else if (ch == HDLC_FLAG) {
-			/* message is finished */
-			dispatch_rx_msg(sercomm.rx.dlci, sercomm.rx.msg);
+			/* message is finished, shall we check the CRC? */
+			if(sercomm.rx.ctrl & HDLC_C_CRC16_BIT &&
+			   sercomm_chk_strip_crc16(sercomm.rx.msg) ){
+				/* for debugging: ignore broken CRC on ECHO DLCI! */
+				if(sercomm.rx.dlci != SC_DLCI_ECHO){
+					msgb_free(sercomm.rx.msg);
+					sercomm.rx.msg = NULL;
+				}
+				sercomm.rx.badcrc++;
+			}
+
+			if(sercomm.rx.msg){
+				dispatch_rx_msg(sercomm.rx.dlci, sercomm.rx.msg);
+				sercomm.rx.rxctr++;
+			}
+
 			/* allocate new buffer */
 			sercomm.rx.msg = NULL;
 			/* start all over again */
diff --git a/src/target/firmware/include/comm/sercomm.h b/src/target/firmware/include/comm/sercomm.h
index c8963a3..4552bff 100644
--- a/src/target/firmware/include/comm/sercomm.h
+++ b/src/target/firmware/include/comm/sercomm.h
@@ -13,6 +13,7 @@
 #define HDLC_C_UI	0x03
 #define HDLC_C_P_BIT	(1 << 4)
 #define HDLC_C_F_BIT	(1 << 4)
+#define HDLC_C_CRC16_BIT (1 << 5)
 
 /* a low sercomm_dlci means high priority.  A high DLCI means low priority */
 enum sercomm_dlci {
@@ -22,7 +23,8 @@ enum sercomm_dlci {
 	SC_DLCI_LOADER  = 9,
 	SC_DLCI_CONSOLE = 10,
 	SC_DLCI_ECHO = 11,
-	_SC_DLCI_MAX
+	_SC_DLCI_MAX		/* make sure that 1<<(SC_DLCI_MAX-1) fits in
+				   sercomm.tx.crc_dlci_mask! */
 };
 
 void sercomm_init(void);
@@ -41,6 +43,10 @@ unsigned int sercomm_tx_queue_depth(uint8_t dlci);
 typedef void (*dlci_cb_t)(uint8_t dlci, struct msgb *msg);
 int sercomm_register_rx_cb(uint8_t dlci, dlci_cb_t cb);
 
+
+/* enable(onoff=1)/disable(=0) CRC16 for frames sent on a given DLCI */
+extern void sercomm_tx_set_crc16(int dlci,int onoff);
+
 /* Driver Interface */
 
 /* fetch one octet of to-be-transmitted serial data. returns 0 if no more data */
@@ -54,4 +60,8 @@ static inline struct msgb *sercomm_alloc_msgb(unsigned int len)
 	return msgb_alloc_headroom(len+4, 4, "sercomm_tx");
 }
 
+/* get counters for god received HDLC frames the ones with bad CRC16 */
+extern void
+sercomm_get_rx_ctr(unsigned int *rxctr,unsigned int *badcrc);
+
 #endif /* _SERCOMM_H */