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
153
154
|
// simpleserial.c
#include "simpleserial.h"
#include <stdint.h>
#include "hal.h"
#include <stdio.h>
typedef struct ss_cmd
{
char c;
unsigned int len;
uint8_t (*fp)(uint8_t*, uint16_t);
} ss_cmd;
#define MAX_SS_CMDS 26
static ss_cmd commands[MAX_SS_CMDS];
static int num_commands = 0;
#define MAX_SS_LEN 256
#define SS_VER_1_0 0
#define SS_VER_1_1 1
static char hex_lookup[16] =
{
'0', '1', '2', '3', '4', '5', '6', '7',
'8', '9', 'A', 'B', 'C', 'D', 'E', 'F'
};
int hex_decode(int len, char* ascii_buf, uint8_t* data_buf)
{
if (len % 2 != 0)
return 1;
for(int i = 0; i < len; i+=2)
{
char n_hi = ascii_buf[i];
char n_lo = ascii_buf[i+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, unsigned int 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;
}
void simpleserial_get(void)
{
char ascii_buf[2*MAX_SS_LEN];
uint8_t data_buf[MAX_SS_LEN];
char c;
// Find which command we're receiving
c = getch();
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;
// Receive characters until we fill the ASCII buffer
int 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;
// Callback
uint8_t ret[1];
ret[0] = commands[cmd].fp(data_buf, i/2);
// Acknowledge (if version is 1.1)
#if SS_VER == SS_VER_1_1
simpleserial_put('z', 1, ret);
#endif
}
void simpleserial_put(char c, int 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');
}
|
