Mercurial > avr_jtag

comparison src/cmd_proto.c @ 1:f7c60e525801

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
cptest and cp_ping.py to test cmd_proto.c.
author Thinker K.F. Li <thinker@branda.to>
date Sat, 21 Feb 2009 20:20:06 +0800
parents
children e410832c3280
comparison
equal deleted inserted replaced
0:a0ce8ebf2f18 1:f7c60e525801
1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <string.h>
4 #include "cmd_proto.h"
5
6 enum {
7 ST_WAIT_MAGIC0,
8 ST_WAIT_MAGIC1,
9 ST_WAIT_SEQ,
10 ST_WAIT_LEN,
11 ST_WAIT_DATA,
12 ST_WAIT_CSUM
13 };
14
15 cmd_proto_t *cmd_proto_new(void) {
16 cmd_proto_t *cp;
17
18 cp = (cmd_proto_t *)malloc(sizeof(cmd_proto_t) + 512);
19 if(cp == NULL)
20 return NULL;
21
22 memset(cp, 0, sizeof(cmd_proto_t));
23 cp->bufs[0] = ((char *)cp) + sizeof(cmd_proto_t);
24 cp->bufs[1] = ((char *)cp) + sizeof(cmd_proto_t) + 256;
25 cp->cmds[0].data = cp->bufs[0] + 1;
26 cp->cmds[1].data = cp->bufs[1] + 1;
27
28 return cp;
29 }
30
31 void cmd_proto_free(cmd_proto_t *cp) {
32 free(cp);
33 }
34
35 cp_cmd_t BAD_CMD;
36 cp_cmd_t CSUM_ERR_CMD;
37
38 static
39 cp_cmd_t *make_cmd(cmd_proto_t *cp) {
40 char *buf = cp->bufs[cp->receiving];
41 cp_cmd_t *cmd;
42 int i;
43
44 if(buf[0] <= 0 || buf[0] >= CPCMD_MAX)
45 return &BAD_CMD;
46
47 cmd = cp->cmds + cp->receiving;
48
49 cmd->seq = cp->seq;
50 cmd->code = buf[0];
51 cmd->data_sz = cp->len - 1;
52
53 cp->receiving ^= 1;
54
55 return cmd;
56 }
57
58 #define CSUM_ADD(csum, c) \
59 do { \
60 csum = ((((csum) << 3) | ((csum) >> 5)) ^ (c)) & 0xff; \
61 } while(0)
62
63 cp_cmd_t *cmd_proto_rcv(cmd_proto_t *cp, int c) {
64 int i, csum;
65 char *buf;
66 cp_cmd_t *cmd = NULL;
67
68 switch(cp->status) {
69 case ST_WAIT_MAGIC0:
70 if(c == CP_MAGIC[0])
71 cp->status = ST_WAIT_MAGIC1;
72 break;
73
74 case ST_WAIT_MAGIC1:
75 if(c == CP_MAGIC[1])
76 cp->status = ST_WAIT_SEQ;
77 else if(c != CP_MAGIC[0])
78 cp->status = ST_WAIT_MAGIC0;
79 break;
80
81 case ST_WAIT_SEQ:
82 cp->seq = c;
83 cp->status = ST_WAIT_LEN;
84 break;
85
86 case ST_WAIT_LEN:
87 cp->len = c;
88 if(c > 0) {
89 cp->status = ST_WAIT_DATA;
90 cp->cnt = 0;
91 } else
92 cp->status = ST_WAIT_CSUM;
93 break;
94
95 case ST_WAIT_DATA:
96 cp->bufs[cp->receiving][cp->cnt++] = c;
97 if(cp->cnt >= cp->len)
98 cp->status = ST_WAIT_CSUM;
99 break;
100
101 case ST_WAIT_CSUM:
102 csum = 0;
103 buf = cp->bufs[cp->receiving];
104 for(i = 0; i < cp->len; i++)
105 CSUM_ADD(csum, buf[i]);
106 CSUM_ADD(csum, c);
107 cp->status = ST_WAIT_MAGIC0;
108 if(csum == 0)
109 cmd = make_cmd(cp);
110 else
111 cmd = &CSUM_ERR_CMD;
112 break;
113
114 default:
115 cp->status = ST_WAIT_MAGIC0;
116 }
117
118 return cmd;
119 }
120
121 int cmd_proto_cmd_fill(char *buf, int seq, cp_ccode_t code, int data_sz) {
122 int i, last = data_sz + CP_CMD_HEAD_SZ;
123 int csum = 0;
124
125 buf[0] = CP_MAGIC[0];
126 buf[1] = CP_MAGIC[1];
127 buf[2] = seq;
128 buf[3] = data_sz + 1;
129 buf[4] = code;
130 for(i = CP_CMD_HEAD_SZ - 1; i < last; i++)
131 CSUM_ADD(csum, buf[i]);
132 CSUM_ADD(csum, 0);
133 buf[i] = csum;
134
135 return data_sz + CP_CMD_OVERHEAD;
136 }