1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
|
// simpleserial.c
#include "simpleserial.h"
#include <stdint.h>
#include "hal.h"
typedef struct ss_cmd
{
char c;
uint32_t len;
uint8_t (*fp)(uint8_t*, uint16_t);
} ss_cmd;
static ss_cmd commands[MAX_SS_CMDS];
static int num_commands = 0;
static char hex_lookup[16] =
{
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
};
int hex_decode(uint32_t len, char* ascii_buf, uint8_t* data_buf)
{
if (len % 2 != 0)
return 1;
for(int i = 0; i < len/2; i++)
{
char n_hi = ascii_buf[i*2];
char n_lo = ascii_buf[i*2+1];
if(n_lo >= '0' && n_lo <= '9')
data_buf[i] = n_lo - '0';
else if(n_lo >= 'A' && n_lo <= 'F')
data_buf[i] = n_lo - 'A' + 10;
else if(n_lo >= 'a' && n_lo <= 'f')
data_buf[i] = n_lo - 'a' + 10;
else
return 1;
if(n_hi >= '0' && n_hi <= '9')
data_buf[i] |= (n_hi - '0') << 4;
else if(n_hi >= 'A' && n_hi <= 'F')
data_buf[i] |= (n_hi - 'A' + 10) << 4;
else if(n_hi >= 'a' && n_hi <= 'f')
data_buf[i] |= (n_hi - 'a' + 10) << 4;
else
return 1;
}
return 0;
}
// Callback function for "v" command.
// This can exist in v1.0 as long as we don't actually send back an ack ("z")
uint8_t check_version(uint8_t* v, uint16_t len)
{
return 0x00;
}
// Set up the SimpleSerial module by preparing internal commands
// This just adds the "v" command for now...
void simpleserial_init()
{
simpleserial_addcmd('v', 0, check_version);
}
int simpleserial_addcmd(char c, uint32_t len, uint8_t (*fp)(uint8_t*, uint16_t))
{
if(num_commands >= MAX_SS_CMDS)
return 1;
if(len > MAX_SS_LEN)
return 1;
commands[num_commands].c = c;
commands[num_commands].len = len;
commands[num_commands].fp = fp;
num_commands++;
return 0;
}
int simpleserial_get(void)
{
char ascii_buf[2*MAX_SS_LEN];
uint8_t data_buf[MAX_SS_LEN];
int ci;
// Find which command we're receiving
ci = getch();
if (ci == -1)
return 0;
char c = (char) ci;
if (c == 'x') {
return 0;
}
int cmd;
for(cmd = 0; cmd < num_commands; cmd++)
{
if(commands[cmd].c == c)
break;
}
// If we didn't find a match, give up right away
if(cmd == num_commands)
return 1;
// Receive characters until we fill the ASCII buffer
uint32_t i = 0;
for(; i < 2*commands[cmd].len; i++)
{
c = getch();
// Check for early \n
if(c == '\n' || c == '\r')
break;
ascii_buf[i] = c;
}
// ASCII buffer is full: convert to bytes
// Check for illegal characters here
if(hex_decode(i, ascii_buf, data_buf))
return 1;
// Callback
uint8_t ret[1];
ret[0] = commands[cmd].fp(data_buf, i/2);
simpleserial_put('z', 1, ret);
return 1;
}
void simpleserial_put(char c, uint32_t size, uint8_t* output)
{
// Write first character
putch(c);
// Write each byte as two nibbles
for(int i = 0; i < size; i++)
{
putch(hex_lookup[output[i] >> 4 ]);
putch(hex_lookup[output[i] & 0xF]);
}
// Write trailing '\n'
putch('\n');
flush();
}
|
