# HG changeset patch # User Thinker K.F. Li # Date 1235218806 -28800 # Node ID f7c60e5258015970f413cae397b844c3a0362472 # Parent a0ce8ebf2f188f8ad91aedda7472460a3a8d610d cptest and cp_ping.py to test cmd_proto.c. diff -r a0ce8ebf2f18 -r f7c60e525801 include/cmd_proto.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/include/cmd_proto.h Sat Feb 21 20:20:06 2009 +0800 @@ -0,0 +1,51 @@ +#ifndef __CMD_PROTO_H_ +#define __CMD_PROTO_H_ + +#define CP_MAGIC "JC" +#define CP_MAX_SZ 0x255 +typedef enum { + CPCMD_DUMMY, + CPCMD_PING, + CPCMD_PONG, + CPCMD_DATA, + CPCMD_ACK, + CPCMD_NAK, + CPCMD_SHIFT_TDI, + CPCMD_SHIFT_TMS, + CPCMD_SHIFT_TDI_TDO, + CPCMD_MAX +} cp_ccode_t; + +typedef struct { + int seq; + cp_ccode_t code; + int data_sz; + char *data; +} cp_cmd_t; + +typedef struct { + cp_cmd_t cmds[2]; + int receiving; + int status; + int seq; + int cnt; + int len; + char *bufs[2]; +} cmd_proto_t; + +extern cmd_proto_t *cmd_proto_new(void); +extern void cmd_proto_free(cmd_proto_t *cp); +extern cp_cmd_t *cmd_proto_rcv(cmd_proto_t *cp, int c); +/* + * \return size of filled command. + */ +extern int cmd_proto_cmd_fill(char *buf, int seq, + cp_ccode_t code, int data_sz); +extern cp_cmd_t BAD_CMD; +extern cp_cmd_t CSUM_ERR_CMD; + +#define CP_CMD_HEAD_SZ 5 +#define CP_CMD_TAIL_SZ 1 +#define CP_CMD_OVERHEAD (CP_CMD_HEAD_SZ + CP_CMD_TAIL_SZ) + +#endif /* __CMD_PROTO_H_ */ diff -r a0ce8ebf2f18 -r f7c60e525801 src/cmd_proto.c --- /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 +#include +#include +#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; +} diff -r a0ce8ebf2f18 -r f7c60e525801 tests/cp_ping.py --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tests/cp_ping.py Sat Feb 21 20:20:06 2009 +0800 @@ -0,0 +1,59 @@ +import struct + +CP_CMD_PING = 1 +CP_CMD_PONG = 2 + +def csum_add(csum, c): + return (((csum << 3) | (csum >> 5)) ^ c) & 0xff + +def cp_ping(seq, data): + csum = 0 + csum = csum_add(0, CP_CMD_PING) + for c in data: + csum = csum_add(csum, ord(c)) + pass + csum = csum_add(csum, 0) + + cmd_str = struct.pack('BBBBB', ord('J'), ord('C'), seq, + len(data) + 1, CP_CMD_PING) + cmd_str = cmd_str + data + chr(csum) + return cmd_str + +def get_reply(fo): + import fcntl, os + import time + + reply = '' + fcntl.fcntl(fo.fileno(), fcntl.F_SETFL, os.O_NONBLOCK) + while True: + try: + s = os.read(fo.fileno(), 256) + except OSError: + time.sleep(0.5) + try: + s = os.read(fo.fileno(), 256) + except OSError: + break + pass + reply = reply + s + pass + return reply + +if __name__ == '__main__': + import sys + + if len(sys.argv) != 2: + print >> sys.stderr, 'Usage: prog ' + sys.exit(1) + pass + + port = sys.argv[1] + + fo = open(port, 'r+b') + cmd_str = cp_ping(0, 'hello') + fo.write(cmd_str) + fo.flush() + + reply = get_reply(fo) + print repr(reply) + pass diff -r a0ce8ebf2f18 -r f7c60e525801 tests/cptest.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tests/cptest.c Sat Feb 21 20:20:06 2009 +0800 @@ -0,0 +1,75 @@ +#include +#include +#include +#include "avriotools.h" +#include "cmd_proto.h" + +#define BAUD_RATE 420000 + +void flash_init(void) { + pin_mode(&PORTB, PINB0, PM_OUTPUT); + pin_mode(&PORTB, PINB1, PM_OUTPUT); + pin_lo(PORTB, PINB0); + pin_lo(PORTB, PINB1); +} + +void flash_led(void) { + pin_hi(PORTB, PINB0); + _delay_ms(50); + pin_lo(PORTB, PINB0); +} + +int main(int argc, char * const argv[]) { + int c; + cmd_proto_t *cp; + cp_cmd_t *cmd; + char buf[255 + CP_CMD_OVERHEAD]; + int sz; + int i; + + flash_init(); + + uart_init(BAUD_RATE); + + cp = cmd_proto_new(); + + while(1) { + uart_getc(c); + cmd = cmd_proto_rcv(cp, c); + if(cmd == NULL) + continue; + + flash_led(); + + if(cmd == &BAD_CMD) { + memcpy(buf + CP_CMD_HEAD_SZ, "bad cmd", 7); + sz = cmd_proto_cmd_fill(buf, cmd->seq, CPCMD_NAK, 7); + for(i = 0; i < sz; i++) + uart_putc(buf[i]); + continue; + } + + if(cmd == &CSUM_ERR_CMD) { + memcpy(buf + CP_CMD_HEAD_SZ, "csum err", 8); + sz = cmd_proto_cmd_fill(buf, cmd->seq, CPCMD_NAK, 8); + for(i = 0; i < sz; i++) + uart_putc(buf[i]); + continue; + } + + switch(cmd->code) { + case CPCMD_PING: + memcpy(buf + CP_CMD_HEAD_SZ, cmd->data, cmd->data_sz); + sz = cmd_proto_cmd_fill(buf, cmd->seq, CPCMD_PONG, cmd->data_sz); + for(i = 0; i < sz; i++) + uart_putc(buf[i]); + break; + + default: + memcpy(buf + CP_CMD_HEAD_SZ, cmd->data, cmd->data_sz); + sz = cmd_proto_cmd_fill(buf, cmd->seq, CPCMD_DATA, cmd->data_sz); + for(i = 0; i < sz; i++) + uart_putc(buf[i]); + } + } +}