NFC library using PN532 to read/write NDEF Messages to NFC tags
Dependents: Seeed_NFC_Shield_write Nucleo_test_nfc
Fork of PN532 by
llcp.cpp
00001 00002 #include "llcp.h" 00003 #include "PN532_debug.h" 00004 00005 // LLCP PDU Type Values 00006 #define PDU_SYMM 0x00 00007 #define PDU_PAX 0x01 00008 #define PDU_CONNECT 0x04 00009 #define PDU_DISC 0x05 00010 #define PDU_CC 0x06 00011 #define PDU_DM 0x07 00012 #define PDU_I 0x0c 00013 #define PDU_RR 0x0d 00014 00015 uint8_t LLCP::SYMM_PDU[2] = {0, 0}; 00016 00017 inline uint8_t getPType(const uint8_t *buf) 00018 { 00019 return ((buf[0] & 0x3) << 2) + (buf[1] >> 6); 00020 } 00021 00022 inline uint8_t getSSAP(const uint8_t *buf) 00023 { 00024 return buf[1] & 0x3f; 00025 } 00026 00027 inline uint8_t getDSAP(const uint8_t *buf) 00028 { 00029 return buf[0] >> 2; 00030 } 00031 00032 int8_t LLCP::activate(uint16_t timeout) 00033 { 00034 return link.activateAsTarget(timeout); 00035 } 00036 00037 int8_t LLCP::waitForConnection(uint16_t timeout) 00038 { 00039 uint8_t type; 00040 00041 ns = 0; 00042 nr = 0; 00043 00044 // Get CONNECT PDU 00045 DMSG("wait for a CONNECT PDU\n"); 00046 do { 00047 if (2 > link.read(headerBuf, headerBufLen)) { 00048 return -1; 00049 } 00050 00051 type = getPType(headerBuf); 00052 if (PDU_CONNECT == type) { 00053 break; 00054 } else if (PDU_SYMM == type) { 00055 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00056 return -2; 00057 } 00058 } else { 00059 return -3; 00060 } 00061 00062 } while (1); 00063 00064 // Put CC PDU 00065 DMSG("put a CC(Connection Complete) PDU to response the CONNECT PDU\n"); 00066 ssap = getDSAP(headerBuf); 00067 dsap = getSSAP(headerBuf); 00068 headerBuf[0] = (dsap << 2) + ((PDU_CC >> 2) & 0x3); 00069 headerBuf[1] = ((PDU_CC & 0x3) << 6) + ssap; 00070 if (!link.write(headerBuf, 2)) { 00071 return -2; 00072 } 00073 00074 return 1; 00075 } 00076 00077 int8_t LLCP::waitForDisconnection(uint16_t timeout) 00078 { 00079 uint8_t type; 00080 00081 // Get DISC PDU 00082 DMSG("wait for a DISC PDU\n"); 00083 do { 00084 if (2 > link.read(headerBuf, headerBufLen)) { 00085 return -1; 00086 } 00087 00088 type = getPType(headerBuf); 00089 if (PDU_DISC == type) { 00090 break; 00091 } else if (PDU_SYMM == type) { 00092 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00093 return -2; 00094 } 00095 } else { 00096 return -3; 00097 } 00098 00099 } while (1); 00100 00101 // Put DM PDU 00102 DMSG("put a DM(Disconnect Mode) PDU to response the DISC PDU\n"); 00103 // ssap = getDSAP(headerBuf); 00104 // dsap = getSSAP(headerBuf); 00105 headerBuf[0] = (dsap << 2) + (PDU_DM >> 2); 00106 headerBuf[1] = ((PDU_DM & 0x3) << 6) + ssap; 00107 if (!link.write(headerBuf, 2)) { 00108 return -2; 00109 } 00110 00111 return 1; 00112 } 00113 00114 int8_t LLCP::connect(uint16_t timeout) 00115 { 00116 uint8_t type; 00117 00118 ns = 0; 00119 nr = 0; 00120 00121 // try to get a SYMM PDU 00122 if (2 > link.read(headerBuf, headerBufLen)) { 00123 return -1; 00124 } 00125 type = getPType(headerBuf); 00126 if (PDU_SYMM != type) { 00127 return -1; 00128 } 00129 00130 dsap = LLCP_DEFAULT_DSAP; 00131 ssap = LLCP_DEFAULT_SSAP; 00132 00133 // put a CONNECT PDU 00134 headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_CONNECT >> 2); 00135 headerBuf[1] = ((PDU_CONNECT & 0x03) << 6) + LLCP_DEFAULT_SSAP; 00136 if (!link.write(headerBuf, 2)) { 00137 return -2; 00138 } 00139 00140 // wait for a CC PDU 00141 DMSG("wait for a CC PDU\n"); 00142 do { 00143 if (2 > link.read(headerBuf, headerBufLen)) { 00144 return -1; 00145 } 00146 00147 type = getPType(headerBuf); 00148 if (PDU_CC == type) { 00149 break; 00150 } else if (PDU_SYMM == type) { 00151 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00152 return -2; 00153 } 00154 } else { 00155 return -3; 00156 } 00157 00158 } while (1); 00159 00160 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00161 return -2; 00162 } 00163 00164 return 1; 00165 } 00166 00167 int8_t LLCP::disconnect(uint16_t timeout) 00168 { 00169 uint8_t type; 00170 00171 // try to get a SYMM PDU 00172 if (2 > link.read(headerBuf, headerBufLen)) { 00173 return -1; 00174 } 00175 type = getPType(headerBuf); 00176 if (PDU_SYMM != type) { 00177 return -1; 00178 } 00179 00180 // put a DISC PDU 00181 headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_DISC >> 2); 00182 headerBuf[1] = ((PDU_DISC & 0x03) << 6) + LLCP_DEFAULT_SSAP; 00183 if (!link.write(headerBuf, 2)) { 00184 return -2; 00185 } 00186 00187 // wait for a DM PDU 00188 DMSG("wait for a DM PDU\n"); 00189 do { 00190 if (2 > link.read(headerBuf, headerBufLen)) { 00191 return -1; 00192 } 00193 00194 type = getPType(headerBuf); 00195 if (PDU_CC == type) { 00196 break; 00197 } else if (PDU_DM == type) { 00198 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00199 return -2; 00200 } 00201 } else { 00202 return -3; 00203 } 00204 00205 } while (1); 00206 00207 return 1; 00208 } 00209 00210 bool LLCP::write(const uint8_t *header, uint8_t hlen, const uint8_t *body, uint8_t blen) 00211 { 00212 uint8_t type; 00213 uint8_t buf[3]; 00214 00215 // Get a SYMM PDU 00216 if (2 != link.read(buf, sizeof(buf))) { 00217 return false; 00218 } 00219 00220 if (headerBufLen < (hlen + 3)) { 00221 return false; 00222 } 00223 00224 for (int8_t i = hlen - 1; i >= 0; i--) { 00225 headerBuf[i + 3] = header[i]; 00226 } 00227 00228 headerBuf[0] = (dsap << 2) + (PDU_I >> 2); 00229 headerBuf[1] = ((PDU_I & 0x3) << 6) + ssap; 00230 headerBuf[2] = (ns << 4) + nr; 00231 if (!link.write(headerBuf, 3 + hlen, body, blen)) { 00232 return false; 00233 } 00234 00235 ns++; 00236 00237 // Get a RR PDU 00238 int16_t status; 00239 do { 00240 status = link.read(headerBuf, headerBufLen); 00241 if (2 > status) { 00242 return false; 00243 } 00244 00245 type = getPType(headerBuf); 00246 if (PDU_RR == type) { 00247 break; 00248 } else if (PDU_SYMM == type) { 00249 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00250 return false; 00251 } 00252 } else { 00253 return false; 00254 } 00255 } while (1); 00256 00257 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00258 return false; 00259 } 00260 00261 return true; 00262 } 00263 00264 int16_t LLCP::read(uint8_t *buf, uint8_t length) 00265 { 00266 uint8_t type; 00267 uint16_t status; 00268 00269 // Get INFO PDU 00270 do { 00271 status = link.read(buf, length); 00272 if (2 > status) { 00273 return -1; 00274 } 00275 00276 type = getPType(buf); 00277 if (PDU_I == type) { 00278 break; 00279 } else if (PDU_SYMM == type) { 00280 if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) { 00281 return -2; 00282 } 00283 } else { 00284 return -3; 00285 } 00286 00287 } while (1); 00288 00289 uint8_t len = status - 3; 00290 ssap = getDSAP(buf); 00291 dsap = getSSAP(buf); 00292 00293 headerBuf[0] = (dsap << 2) + (PDU_RR >> 2); 00294 headerBuf[1] = ((PDU_RR & 0x3) << 6) + ssap; 00295 headerBuf[2] = (buf[2] >> 4) + 1; 00296 if (!link.write(headerBuf, 3)) { 00297 return -2; 00298 } 00299 00300 for (uint8_t i = 0; i < len; i++) { 00301 buf[i] = buf[i + 3]; 00302 } 00303 00304 nr++; 00305 00306 return len; 00307 }
Generated on Tue Jul 12 2022 21:59:39 by 1.7.2