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     mode = 1;
00042     ns = 0;
00043     nr = 0;
00044 
00045     // Get CONNECT PDU
00046     DMSG("wait for a CONNECT PDU\n");
00047     do {
00048         if (2 > link.read(headerBuf, headerBufLen)) {
00049             return -1;
00050         }
00051 
00052         type = getPType(headerBuf);
00053         if (PDU_CONNECT == type) {
00054             break;
00055         } else if (PDU_SYMM == type) {
00056             if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00057                 return -2;
00058             }
00059         } else {
00060             return -3;
00061         }
00062 
00063     } while (1);
00064 
00065     // Put CC PDU
00066     DMSG("put a CC(Connection Complete) PDU to response the CONNECT PDU\n");
00067     ssap = getDSAP(headerBuf);
00068     dsap = getSSAP(headerBuf);
00069     headerBuf[0] = (dsap << 2) + ((PDU_CC >> 2) & 0x3);
00070     headerBuf[1] = ((PDU_CC & 0x3) << 6) + ssap;
00071     if (!link.write(headerBuf, 2)) {
00072         return -2;
00073     }
00074 
00075     return 1;
00076 }
00077 
00078 int8_t LLCP::waitForDisconnection(uint16_t timeout)
00079 {
00080     uint8_t type;
00081 
00082     // Get DISC PDU
00083     DMSG("wait for a DISC PDU\n");
00084     do {
00085         if (2 > link.read(headerBuf, headerBufLen)) {
00086             return -1;
00087         }
00088 
00089         type = getPType(headerBuf);
00090         if (PDU_DISC == type) {
00091             break;
00092         } else if (PDU_SYMM == type) {
00093             if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00094                 return -2;
00095             }
00096         } else {
00097             return -3;
00098         }
00099 
00100     } while (1);
00101 
00102     // Put DM PDU
00103     DMSG("put a DM(Disconnect Mode) PDU to response the DISC PDU\n");
00104     // ssap = getDSAP(headerBuf);
00105     // dsap = getSSAP(headerBuf);
00106     headerBuf[0] = (dsap << 2) + (PDU_DM >> 2);
00107     headerBuf[1] = ((PDU_DM & 0x3) << 6) + ssap;
00108     if (!link.write(headerBuf, 2)) {
00109         return -2;
00110     }
00111 
00112     return 1;
00113 }
00114 
00115 int8_t LLCP::connect(uint16_t timeout)
00116 {
00117     uint8_t type;
00118 
00119     mode = 0;
00120     dsap = LLCP_DEFAULT_DSAP;
00121     ssap = LLCP_DEFAULT_SSAP;
00122     ns = 0;
00123     nr = 0;
00124 
00125     // try to get a SYMM PDU
00126     if (2 > link.read(headerBuf, headerBufLen)) {
00127         return -1;
00128     }
00129     type = getPType(headerBuf);
00130     if (PDU_SYMM != type) {
00131         return -1;
00132     }
00133 
00134     // put a CONNECT PDU
00135     headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_CONNECT >> 2);
00136     headerBuf[1] = ((PDU_CONNECT & 0x03) << 6) + LLCP_DEFAULT_SSAP;
00137     uint8_t body[] = "  urn:nfc:sn:snep";
00138     body[0] = 0x06;
00139     body[1] = sizeof(body) - 2 - 1;
00140     if (!link.write(headerBuf, 2, body, sizeof(body) - 1)) {
00141         return -2;
00142     }
00143 
00144     // wait for a CC PDU
00145     DMSG("wait for a CC PDU\n");
00146     do {
00147         if (2 > link.read(headerBuf, headerBufLen)) {
00148             return -1;
00149         }
00150 
00151         type = getPType(headerBuf);
00152         if (PDU_CC == type) {
00153             break;
00154         } else if (PDU_SYMM == type) {
00155             if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00156                 return -2;
00157             }
00158         } else {
00159             return -3;
00160         }
00161 
00162     } while (1);
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     if (mode) {
00216         // Get a SYMM PDU
00217         if (2 != link.read(buf, sizeof(buf))) {
00218             return false;
00219         }
00220     }
00221 
00222     if (headerBufLen < (hlen + 3)) {
00223         return false;
00224     }
00225 
00226     for (int8_t i = hlen - 1; i >= 0; i--) {
00227         headerBuf[i + 3] = header[i];
00228     }
00229 
00230     headerBuf[0] = (dsap << 2) + (PDU_I >> 2);
00231     headerBuf[1] = ((PDU_I & 0x3) << 6) + ssap;
00232     headerBuf[2] = (ns << 4) + nr;
00233     if (!link.write(headerBuf, 3 + hlen, body, blen)) {
00234         return false;
00235     }
00236 
00237     ns++;
00238 
00239     // Get a RR PDU
00240     int16_t status;
00241     do {
00242         status = link.read(headerBuf, headerBufLen);
00243         if (2 > status) {
00244             return false;
00245         }
00246 
00247         type = getPType(headerBuf);
00248         if (PDU_RR == type) {
00249             break;
00250         } else if (PDU_SYMM == type) {
00251             if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00252                 return false;
00253             }
00254         } else {
00255             return false;
00256         }
00257     } while (1);
00258 
00259     if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00260         return false;
00261     }
00262 
00263     return true;
00264 }
00265 
00266 int16_t LLCP::read(uint8_t *buf, uint8_t length)
00267 {
00268     uint8_t type;
00269     uint16_t status;
00270 
00271     // Get INFO PDU
00272     do {
00273         status = link.read(buf, length);
00274         if (2 > status) {
00275             return -1;
00276         }
00277 
00278         type = getPType(buf);
00279         if (PDU_I == type) {
00280             break;
00281         } else if (PDU_SYMM == type) {
00282             if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
00283                 return -2;
00284             }
00285         } else {
00286             return -3;
00287         }
00288 
00289     } while (1);
00290 
00291     uint8_t len = status - 3;
00292     ssap = getDSAP(buf);
00293     dsap = getSSAP(buf);
00294 
00295     headerBuf[0] = (dsap << 2) + (PDU_RR >> 2);
00296     headerBuf[1] = ((PDU_RR & 0x3) << 6) + ssap;
00297     headerBuf[2] = (buf[2] >> 4) + 1;
00298     if (!link.write(headerBuf, 3)) {
00299         return -2;
00300     }
00301 
00302     for (uint8_t i = 0; i < len; i++) {
00303         buf[i] = buf[i + 3];
00304     }
00305 
00306     nr++;
00307 
00308     return len;
00309 }