--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/cmd_proto.c	Sat Feb 21 20:20:06 2009 +0800
@@ -0,0 +1,136 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "cmd_proto.h"
+
+enum {
+    ST_WAIT_MAGIC0,
+    ST_WAIT_MAGIC1,
+    ST_WAIT_SEQ,
+    ST_WAIT_LEN,
+    ST_WAIT_DATA,
+    ST_WAIT_CSUM
+};
+
+cmd_proto_t *cmd_proto_new(void) {
+    cmd_proto_t *cp;
+
+    cp = (cmd_proto_t *)malloc(sizeof(cmd_proto_t) + 512);
+    if(cp == NULL)
+	return NULL;
+    
+    memset(cp, 0, sizeof(cmd_proto_t));
+    cp->bufs[0] = ((char *)cp) + sizeof(cmd_proto_t);
+    cp->bufs[1] = ((char *)cp) + sizeof(cmd_proto_t) + 256;
+    cp->cmds[0].data = cp->bufs[0] + 1;
+    cp->cmds[1].data = cp->bufs[1] + 1;
+
+    return cp;
+}
+
+void cmd_proto_free(cmd_proto_t *cp) {
+    free(cp);
+}
+
+cp_cmd_t BAD_CMD;
+cp_cmd_t CSUM_ERR_CMD;
+
+static
+cp_cmd_t *make_cmd(cmd_proto_t *cp) {
+    char *buf = cp->bufs[cp->receiving];
+    cp_cmd_t *cmd;
+    int i;
+
+    if(buf[0] <= 0 || buf[0] >= CPCMD_MAX)
+	return &BAD_CMD;
+    
+    cmd = cp->cmds + cp->receiving;
+
+    cmd->seq = cp->seq;
+    cmd->code = buf[0];
+    cmd->data_sz = cp->len - 1;
+
+    cp->receiving ^= 1;
+    
+    return cmd;
+}
+
+#define CSUM_ADD(csum, c)					\
+    do {							\
+	csum = ((((csum) << 3) | ((csum) >> 5)) ^ (c)) & 0xff;	\
+    } while(0)
+
+cp_cmd_t *cmd_proto_rcv(cmd_proto_t *cp, int c) {
+    int i, csum;
+    char *buf;
+    cp_cmd_t *cmd = NULL;
+    
+    switch(cp->status) {
+    case ST_WAIT_MAGIC0:
+	if(c == CP_MAGIC[0])
+	    cp->status = ST_WAIT_MAGIC1;
+	break;
+	
+    case ST_WAIT_MAGIC1:
+	if(c == CP_MAGIC[1])
+	    cp->status = ST_WAIT_SEQ;
+	else if(c != CP_MAGIC[0])
+	    cp->status = ST_WAIT_MAGIC0;
+	break;
+
+    case ST_WAIT_SEQ:
+	cp->seq = c;
+	cp->status = ST_WAIT_LEN;
+	break;
+	
+    case ST_WAIT_LEN:
+	cp->len = c;
+	if(c > 0) {
+	    cp->status = ST_WAIT_DATA;
+	    cp->cnt = 0;
+	} else
+	    cp->status = ST_WAIT_CSUM;
+	break;
+	
+    case ST_WAIT_DATA:
+	cp->bufs[cp->receiving][cp->cnt++] = c;
+	if(cp->cnt >= cp->len)
+	    cp->status = ST_WAIT_CSUM;
+	break;
+	
+    case ST_WAIT_CSUM:
+	csum = 0;
+	buf = cp->bufs[cp->receiving];
+	for(i = 0; i < cp->len; i++)
+	    CSUM_ADD(csum, buf[i]);
+	CSUM_ADD(csum, c);
+	cp->status = ST_WAIT_MAGIC0;
+	if(csum == 0)
+	    cmd = make_cmd(cp);
+	else
+	    cmd = &CSUM_ERR_CMD;
+	break;
+	
+    default:
+	cp->status = ST_WAIT_MAGIC0;
+    }
+
+    return cmd;
+}
+
+int cmd_proto_cmd_fill(char *buf, int seq, cp_ccode_t code, int data_sz) {
+    int i, last = data_sz + CP_CMD_HEAD_SZ;
+    int csum = 0;
+    
+    buf[0] = CP_MAGIC[0];
+    buf[1] = CP_MAGIC[1];
+    buf[2] = seq;
+    buf[3] = data_sz + 1;
+    buf[4] = code;
+    for(i = CP_CMD_HEAD_SZ - 1; i < last; i++)
+	CSUM_ADD(csum, buf[i]);
+    CSUM_ADD(csum, 0);
+    buf[i] = csum;
+    
+    return data_sz + CP_CMD_OVERHEAD;
+}