HTM / PN532_MK20

LEER TAG

Dependents:   NFC_HTM_READ EMULAR_TAGS Escribir_tag NFC_HTM_READ-WRITE

llcp.cpp@1:a549ef8b142a, 2015-04-24 (annotated)

Committer:
mauroar211
Date:
Fri Apr 24 19:02:44 2015 +0000
Revision:
1:a549ef8b142a
Parent:
0:b805b487fbef 
SE RESTAURARON LOS ARCHIVO DE LA LIBRER?A;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mauroar211 0:b805b487fbef 1
mauroar211 0:b805b487fbef 2 #include "llcp.h"
mauroar211 0:b805b487fbef 3 #include "PN532_debug.h"
mauroar211 0:b805b487fbef 4
mauroar211 0:b805b487fbef 5 // LLCP PDU Type Values
mauroar211 0:b805b487fbef 6 #define PDU_SYMM 0x00
mauroar211 0:b805b487fbef 7 #define PDU_PAX 0x01
mauroar211 0:b805b487fbef 8 #define PDU_CONNECT 0x04
mauroar211 0:b805b487fbef 9 #define PDU_DISC 0x05
mauroar211 0:b805b487fbef 10 #define PDU_CC 0x06
mauroar211 0:b805b487fbef 11 #define PDU_DM 0x07
mauroar211 0:b805b487fbef 12 #define PDU_I 0x0c
mauroar211 0:b805b487fbef 13 #define PDU_RR 0x0d
mauroar211 0:b805b487fbef 14
mauroar211 0:b805b487fbef 15 uint8_t LLCP::SYMM_PDU[2] = {0, 0};
mauroar211 0:b805b487fbef 16
mauroar211 0:b805b487fbef 17 inline uint8_t getPType(const uint8_t *buf)
mauroar211 0:b805b487fbef 18 {
mauroar211 0:b805b487fbef 19 return ((buf[0] & 0x3) << 2) + (buf[1] >> 6);
mauroar211 0:b805b487fbef 20 }
mauroar211 0:b805b487fbef 21
mauroar211 0:b805b487fbef 22 inline uint8_t getSSAP(const uint8_t *buf)
mauroar211 0:b805b487fbef 23 {
mauroar211 0:b805b487fbef 24 return buf[1] & 0x3f;
mauroar211 0:b805b487fbef 25 }
mauroar211 0:b805b487fbef 26
mauroar211 0:b805b487fbef 27 inline uint8_t getDSAP(const uint8_t *buf)
mauroar211 0:b805b487fbef 28 {
mauroar211 0:b805b487fbef 29 return buf[0] >> 2;
mauroar211 0:b805b487fbef 30 }
mauroar211 0:b805b487fbef 31
mauroar211 0:b805b487fbef 32 int8_t LLCP::activate(uint16_t timeout)
mauroar211 0:b805b487fbef 33 {
mauroar211 0:b805b487fbef 34 return link.activateAsTarget(timeout);
mauroar211 0:b805b487fbef 35 }
mauroar211 0:b805b487fbef 36
mauroar211 0:b805b487fbef 37 int8_t LLCP::waitForConnection(uint16_t timeout)
mauroar211 0:b805b487fbef 38 {
mauroar211 0:b805b487fbef 39 uint8_t type;
mauroar211 0:b805b487fbef 40
mauroar211 0:b805b487fbef 41 ns = 0;
mauroar211 0:b805b487fbef 42 nr = 0;
mauroar211 0:b805b487fbef 43
mauroar211 0:b805b487fbef 44 // Get CONNECT PDU
mauroar211 0:b805b487fbef 45 DMSG("wait for a CONNECT PDU\n");
mauroar211 0:b805b487fbef 46 do {
mauroar211 0:b805b487fbef 47 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 48 return -1;
mauroar211 0:b805b487fbef 49 }
mauroar211 0:b805b487fbef 50
mauroar211 0:b805b487fbef 51 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 52 if (PDU_CONNECT == type) {
mauroar211 0:b805b487fbef 53 break;
mauroar211 0:b805b487fbef 54 } else if (PDU_SYMM == type) {
mauroar211 0:b805b487fbef 55 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 56 return -2;
mauroar211 0:b805b487fbef 57 }
mauroar211 0:b805b487fbef 58 } else {
mauroar211 0:b805b487fbef 59 return -3;
mauroar211 0:b805b487fbef 60 }
mauroar211 0:b805b487fbef 61
mauroar211 0:b805b487fbef 62 } while (1);
mauroar211 0:b805b487fbef 63
mauroar211 0:b805b487fbef 64 // Put CC PDU
mauroar211 0:b805b487fbef 65 DMSG("put a CC(Connection Complete) PDU to response the CONNECT PDU\n");
mauroar211 0:b805b487fbef 66 ssap = getDSAP(headerBuf);
mauroar211 0:b805b487fbef 67 dsap = getSSAP(headerBuf);
mauroar211 0:b805b487fbef 68 headerBuf[0] = (dsap << 2) + ((PDU_CC >> 2) & 0x3);
mauroar211 0:b805b487fbef 69 headerBuf[1] = ((PDU_CC & 0x3) << 6) + ssap;
mauroar211 0:b805b487fbef 70 if (!link.write(headerBuf, 2)) {
mauroar211 0:b805b487fbef 71 return -2;
mauroar211 0:b805b487fbef 72 }
mauroar211 0:b805b487fbef 73
mauroar211 0:b805b487fbef 74 return 1;
mauroar211 0:b805b487fbef 75 }
mauroar211 0:b805b487fbef 76
mauroar211 0:b805b487fbef 77 int8_t LLCP::waitForDisconnection(uint16_t timeout)
mauroar211 0:b805b487fbef 78 {
mauroar211 0:b805b487fbef 79 uint8_t type;
mauroar211 0:b805b487fbef 80
mauroar211 0:b805b487fbef 81 // Get DISC PDU
mauroar211 0:b805b487fbef 82 DMSG("wait for a DISC PDU\n");
mauroar211 0:b805b487fbef 83 do {
mauroar211 0:b805b487fbef 84 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 85 return -1;
mauroar211 0:b805b487fbef 86 }
mauroar211 0:b805b487fbef 87
mauroar211 0:b805b487fbef 88 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 89 if (PDU_DISC == type) {
mauroar211 0:b805b487fbef 90 break;
mauroar211 0:b805b487fbef 91 } else if (PDU_SYMM == type) {
mauroar211 0:b805b487fbef 92 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 93 return -2;
mauroar211 0:b805b487fbef 94 }
mauroar211 0:b805b487fbef 95 } else {
mauroar211 0:b805b487fbef 96 return -3;
mauroar211 0:b805b487fbef 97 }
mauroar211 0:b805b487fbef 98
mauroar211 0:b805b487fbef 99 } while (1);
mauroar211 0:b805b487fbef 100
mauroar211 0:b805b487fbef 101 // Put DM PDU
mauroar211 0:b805b487fbef 102 DMSG("put a DM(Disconnect Mode) PDU to response the DISC PDU\n");
mauroar211 0:b805b487fbef 103 // ssap = getDSAP(headerBuf);
mauroar211 0:b805b487fbef 104 // dsap = getSSAP(headerBuf);
mauroar211 0:b805b487fbef 105 headerBuf[0] = (dsap << 2) + (PDU_DM >> 2);
mauroar211 0:b805b487fbef 106 headerBuf[1] = ((PDU_DM & 0x3) << 6) + ssap;
mauroar211 0:b805b487fbef 107 if (!link.write(headerBuf, 2)) {
mauroar211 0:b805b487fbef 108 return -2;
mauroar211 0:b805b487fbef 109 }
mauroar211 0:b805b487fbef 110
mauroar211 0:b805b487fbef 111 return 1;
mauroar211 0:b805b487fbef 112 }
mauroar211 0:b805b487fbef 113
mauroar211 0:b805b487fbef 114 int8_t LLCP::connect(uint16_t timeout)
mauroar211 0:b805b487fbef 115 {
mauroar211 0:b805b487fbef 116 uint8_t type;
mauroar211 0:b805b487fbef 117
mauroar211 0:b805b487fbef 118 ns = 0;
mauroar211 0:b805b487fbef 119 nr = 0;
mauroar211 0:b805b487fbef 120
mauroar211 0:b805b487fbef 121 // try to get a SYMM PDU
mauroar211 0:b805b487fbef 122 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 123 return -1;
mauroar211 0:b805b487fbef 124 }
mauroar211 0:b805b487fbef 125 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 126 if (PDU_SYMM != type) {
mauroar211 0:b805b487fbef 127 return -1;
mauroar211 0:b805b487fbef 128 }
mauroar211 0:b805b487fbef 129
mauroar211 0:b805b487fbef 130 dsap = LLCP_DEFAULT_DSAP;
mauroar211 0:b805b487fbef 131 ssap = LLCP_DEFAULT_SSAP;
mauroar211 0:b805b487fbef 132
mauroar211 0:b805b487fbef 133 // put a CONNECT PDU
mauroar211 0:b805b487fbef 134 headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_CONNECT >> 2);
mauroar211 0:b805b487fbef 135 headerBuf[1] = ((PDU_CONNECT & 0x03) << 6) + LLCP_DEFAULT_SSAP;
mauroar211 0:b805b487fbef 136 if (!link.write(headerBuf, 2)) {
mauroar211 0:b805b487fbef 137 return -2;
mauroar211 0:b805b487fbef 138 }
mauroar211 0:b805b487fbef 139
mauroar211 0:b805b487fbef 140 // wait for a CC PDU
mauroar211 0:b805b487fbef 141 DMSG("wait for a CC PDU\n");
mauroar211 0:b805b487fbef 142 do {
mauroar211 0:b805b487fbef 143 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 144 return -1;
mauroar211 0:b805b487fbef 145 }
mauroar211 0:b805b487fbef 146
mauroar211 0:b805b487fbef 147 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 148 if (PDU_CC == type) {
mauroar211 0:b805b487fbef 149 break;
mauroar211 0:b805b487fbef 150 } else if (PDU_SYMM == type) {
mauroar211 0:b805b487fbef 151 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 152 return -2;
mauroar211 0:b805b487fbef 153 }
mauroar211 0:b805b487fbef 154 } else {
mauroar211 0:b805b487fbef 155 return -3;
mauroar211 0:b805b487fbef 156 }
mauroar211 0:b805b487fbef 157
mauroar211 0:b805b487fbef 158 } while (1);
mauroar211 0:b805b487fbef 159
mauroar211 0:b805b487fbef 160 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 161 return -2;
mauroar211 0:b805b487fbef 162 }
mauroar211 0:b805b487fbef 163
mauroar211 0:b805b487fbef 164 return 1;
mauroar211 0:b805b487fbef 165 }
mauroar211 0:b805b487fbef 166
mauroar211 0:b805b487fbef 167 int8_t LLCP::disconnect(uint16_t timeout)
mauroar211 0:b805b487fbef 168 {
mauroar211 0:b805b487fbef 169 uint8_t type;
mauroar211 0:b805b487fbef 170
mauroar211 0:b805b487fbef 171 // try to get a SYMM PDU
mauroar211 0:b805b487fbef 172 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 173 return -1;
mauroar211 0:b805b487fbef 174 }
mauroar211 0:b805b487fbef 175 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 176 if (PDU_SYMM != type) {
mauroar211 0:b805b487fbef 177 return -1;
mauroar211 0:b805b487fbef 178 }
mauroar211 0:b805b487fbef 179
mauroar211 0:b805b487fbef 180 // put a DISC PDU
mauroar211 0:b805b487fbef 181 headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_DISC >> 2);
mauroar211 0:b805b487fbef 182 headerBuf[1] = ((PDU_DISC & 0x03) << 6) + LLCP_DEFAULT_SSAP;
mauroar211 0:b805b487fbef 183 if (!link.write(headerBuf, 2)) {
mauroar211 0:b805b487fbef 184 return -2;
mauroar211 0:b805b487fbef 185 }
mauroar211 0:b805b487fbef 186
mauroar211 0:b805b487fbef 187 // wait for a DM PDU
mauroar211 0:b805b487fbef 188 DMSG("wait for a DM PDU\n");
mauroar211 0:b805b487fbef 189 do {
mauroar211 0:b805b487fbef 190 if (2 > link.read(headerBuf, headerBufLen)) {
mauroar211 0:b805b487fbef 191 return -1;
mauroar211 0:b805b487fbef 192 }
mauroar211 0:b805b487fbef 193
mauroar211 0:b805b487fbef 194 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 195 if (PDU_CC == type) {
mauroar211 0:b805b487fbef 196 break;
mauroar211 0:b805b487fbef 197 } else if (PDU_DM == type) {
mauroar211 0:b805b487fbef 198 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 199 return -2;
mauroar211 0:b805b487fbef 200 }
mauroar211 0:b805b487fbef 201 } else {
mauroar211 0:b805b487fbef 202 return -3;
mauroar211 0:b805b487fbef 203 }
mauroar211 0:b805b487fbef 204
mauroar211 0:b805b487fbef 205 } while (1);
mauroar211 0:b805b487fbef 206
mauroar211 0:b805b487fbef 207 return 1;
mauroar211 0:b805b487fbef 208 }
mauroar211 0:b805b487fbef 209
mauroar211 0:b805b487fbef 210 bool LLCP::write(const uint8_t *header, uint8_t hlen, const uint8_t *body, uint8_t blen)
mauroar211 0:b805b487fbef 211 {
mauroar211 0:b805b487fbef 212 uint8_t type;
mauroar211 0:b805b487fbef 213 uint8_t buf[3];
mauroar211 0:b805b487fbef 214
mauroar211 0:b805b487fbef 215 // Get a SYMM PDU
mauroar211 0:b805b487fbef 216 if (2 != link.read(buf, sizeof(buf))) {
mauroar211 0:b805b487fbef 217 return false;
mauroar211 0:b805b487fbef 218 }
mauroar211 0:b805b487fbef 219
mauroar211 0:b805b487fbef 220 if (headerBufLen < (hlen + 3)) {
mauroar211 0:b805b487fbef 221 return false;
mauroar211 0:b805b487fbef 222 }
mauroar211 0:b805b487fbef 223
mauroar211 0:b805b487fbef 224 for (int8_t i = hlen - 1; i >= 0; i--) {
mauroar211 0:b805b487fbef 225 headerBuf[i + 3] = header[i];
mauroar211 0:b805b487fbef 226 }
mauroar211 0:b805b487fbef 227
mauroar211 0:b805b487fbef 228 headerBuf[0] = (dsap << 2) + (PDU_I >> 2);
mauroar211 0:b805b487fbef 229 headerBuf[1] = ((PDU_I & 0x3) << 6) + ssap;
mauroar211 0:b805b487fbef 230 headerBuf[2] = (ns << 4) + nr;
mauroar211 0:b805b487fbef 231 if (!link.write(headerBuf, 3 + hlen, body, blen)) {
mauroar211 0:b805b487fbef 232 return false;
mauroar211 0:b805b487fbef 233 }
mauroar211 0:b805b487fbef 234
mauroar211 0:b805b487fbef 235 ns++;
mauroar211 0:b805b487fbef 236
mauroar211 0:b805b487fbef 237 // Get a RR PDU
mauroar211 0:b805b487fbef 238 int16_t status;
mauroar211 0:b805b487fbef 239 do {
mauroar211 0:b805b487fbef 240 status = link.read(headerBuf, headerBufLen);
mauroar211 0:b805b487fbef 241 if (2 > status) {
mauroar211 0:b805b487fbef 242 return false;
mauroar211 0:b805b487fbef 243 }
mauroar211 0:b805b487fbef 244
mauroar211 0:b805b487fbef 245 type = getPType(headerBuf);
mauroar211 0:b805b487fbef 246 if (PDU_RR == type) {
mauroar211 0:b805b487fbef 247 break;
mauroar211 0:b805b487fbef 248 } else if (PDU_SYMM == type) {
mauroar211 0:b805b487fbef 249 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 250 return false;
mauroar211 0:b805b487fbef 251 }
mauroar211 0:b805b487fbef 252 } else {
mauroar211 0:b805b487fbef 253 return false;
mauroar211 0:b805b487fbef 254 }
mauroar211 0:b805b487fbef 255 } while (1);
mauroar211 0:b805b487fbef 256
mauroar211 0:b805b487fbef 257 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 258 return false;
mauroar211 0:b805b487fbef 259 }
mauroar211 0:b805b487fbef 260
mauroar211 0:b805b487fbef 261 return true;
mauroar211 0:b805b487fbef 262 }
mauroar211 0:b805b487fbef 263
mauroar211 0:b805b487fbef 264 int16_t LLCP::read(uint8_t *buf, uint8_t length)
mauroar211 0:b805b487fbef 265 {
mauroar211 0:b805b487fbef 266 uint8_t type;
mauroar211 0:b805b487fbef 267 uint16_t status;
mauroar211 0:b805b487fbef 268
mauroar211 0:b805b487fbef 269 // Get INFO PDU
mauroar211 0:b805b487fbef 270 do {
mauroar211 0:b805b487fbef 271 status = link.read(buf, length);
mauroar211 0:b805b487fbef 272 if (2 > status) {
mauroar211 0:b805b487fbef 273 return -1;
mauroar211 0:b805b487fbef 274 }
mauroar211 0:b805b487fbef 275
mauroar211 0:b805b487fbef 276 type = getPType(buf);
mauroar211 0:b805b487fbef 277 if (PDU_I == type) {
mauroar211 0:b805b487fbef 278 break;
mauroar211 0:b805b487fbef 279 } else if (PDU_SYMM == type) {
mauroar211 0:b805b487fbef 280 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
mauroar211 0:b805b487fbef 281 return -2;
mauroar211 0:b805b487fbef 282 }
mauroar211 0:b805b487fbef 283 } else {
mauroar211 0:b805b487fbef 284 return -3;
mauroar211 0:b805b487fbef 285 }
mauroar211 0:b805b487fbef 286
mauroar211 0:b805b487fbef 287 } while (1);
mauroar211 0:b805b487fbef 288
mauroar211 0:b805b487fbef 289 uint8_t len = status - 3;
mauroar211 0:b805b487fbef 290 ssap = getDSAP(buf);
mauroar211 0:b805b487fbef 291 dsap = getSSAP(buf);
mauroar211 0:b805b487fbef 292
mauroar211 0:b805b487fbef 293 headerBuf[0] = (dsap << 2) + (PDU_RR >> 2);
mauroar211 0:b805b487fbef 294 headerBuf[1] = ((PDU_RR & 0x3) << 6) + ssap;
mauroar211 0:b805b487fbef 295 headerBuf[2] = (buf[2] >> 4) + 1;
mauroar211 0:b805b487fbef 296 if (!link.write(headerBuf, 3)) {
mauroar211 0:b805b487fbef 297 return -2;
mauroar211 0:b805b487fbef 298 }
mauroar211 0:b805b487fbef 299
mauroar211 0:b805b487fbef 300 for (uint8_t i = 0; i < len; i++) {
mauroar211 0:b805b487fbef 301 buf[i] = buf[i + 3];
mauroar211 0:b805b487fbef 302 }
mauroar211 0:b805b487fbef 303
mauroar211 0:b805b487fbef 304 nr++;
mauroar211 0:b805b487fbef 305
mauroar211 0:b805b487fbef 306 return len;
mauroar211 0:b805b487fbef 307 }