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 }