Aniket Udare / LibPN532_NFC

Forked from LibPN532

Dependents:   NFC_Secure_Access NFC_Secure_Access

Fork of LibPN532 by dotnfc Tang

PN532/llcp.cpp@2:9a2ab3fa7862, 2018-04-22 (annotated)

Committer:
udareaniket
Date:
Sun Apr 22 23:29:20 2018 +0000
Revision:
2:9a2ab3fa7862
Parent:
0:db8030e71f55 
Initial commit;

Who changed what in which revision?

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