snep.cpp
2.25 KB
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
#include "snep.h"
#include "PN532_debug.h"
int8_t SNEP::write(const uint8_t *buf, uint8_t len, uint16_t timeout)
{
if (0 >= llcp.activate(timeout)) {
DMSG("failed to activate PN532 as a target\n");
return -1;
}
if (0 >= llcp.connect(timeout)) {
DMSG("failed to set up a connection\n");
return -2;
}
// response a success SNEP message
headerBuf[0] = SNEP_DEFAULT_VERSION;
headerBuf[1] = SNEP_REQUEST_PUT;
headerBuf[2] = 0;
headerBuf[3] = 0;
headerBuf[4] = 0;
headerBuf[5] = len;
if (0 >= llcp.write(headerBuf, 6, buf, len)) {
return -3;
}
uint8_t rbuf[16];
if (6 > llcp.read(rbuf, sizeof(rbuf))) {
return -4;
}
// check SNEP version
if (SNEP_DEFAULT_VERSION != rbuf[0]) {
DMSG("The received SNEP message's major version is different\n");
// To-do: send Unsupported Version response
return -4;
}
// expect a put request
if (SNEP_RESPONSE_SUCCESS != rbuf[1]) {
DMSG("Expect a success response\n");
return -4;
}
llcp.disconnect(timeout);
return 1;
}
int16_t SNEP::read(uint8_t *buf, uint8_t len, uint16_t timeout)
{
if (0 >= llcp.activate(timeout)) {
DMSG("failed to activate PN532 as a target\n");
return -1;
}
if (0 >= llcp.waitForConnection(timeout)) {
DMSG("failed to set up a connection\n");
return -2;
}
uint16_t status = llcp.read(buf, len);
if (6 > status) {
return -3;
}
// check SNEP version
if (SNEP_DEFAULT_VERSION != buf[0]) {
DMSG("The received SNEP message's major version is different\n");
// To-do: send Unsupported Version response
return -4;
}
// expect a put request
if (SNEP_REQUEST_PUT != buf[1]) {
DMSG("Expect a put request\n");
return -4;
}
// check message's length
uint32_t length = (buf[2] << 24) + (buf[3] << 16) + (buf[4] << 8) + buf[5];
// length should not be more than 244 (header + body < 255, header = 6 + 3 + 2)
if (length > (status - 6)) {
DMSG("The SNEP message is too large: ");
DMSG_INT(length);
DMSG_INT(status - 6);
DMSG("\n");
return -4;
}
for (uint8_t i = 0; i < length; i++) {
buf[i] = buf[i + 6];
}
// response a success SNEP message
headerBuf[0] = SNEP_DEFAULT_VERSION;
headerBuf[1] = SNEP_RESPONSE_SUCCESS;
headerBuf[2] = 0;
headerBuf[3] = 0;
headerBuf[4] = 0;
headerBuf[5] = 0;
llcp.write(headerBuf, 6);
return length;
}