Diff: PN532/llcp.cpp

Revision:

0:db8030e71f55

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PN532/llcp.cpp	Tue Sep 13 06:01:19 2016 +0000
@@ -0,0 +1,309 @@
+
+#include "llcp.h"
+#include "PN532_debug.h"
+
+// LLCP PDU Type Values
+#define PDU_SYMM    0x00
+#define PDU_PAX     0x01
+#define PDU_CONNECT 0x04
+#define PDU_DISC    0x05
+#define PDU_CC      0x06
+#define PDU_DM      0x07
+#define PDU_I       0x0c
+#define PDU_RR      0x0d
+
+uint8_t LLCP::SYMM_PDU[2] = {0, 0};
+
+inline uint8_t getPType(const uint8_t *buf)
+{
+    return ((buf[0] & 0x3) << 2) + (buf[1] >> 6);
+}
+
+inline uint8_t getSSAP(const uint8_t *buf)
+{
+    return  buf[1] & 0x3f;
+}
+
+inline uint8_t getDSAP(const uint8_t *buf)
+{
+    return buf[0] >> 2;
+}
+
+int8_t LLCP::activate(uint16_t timeout)
+{
+    return link.activateAsTarget(timeout);
+}
+
+int8_t LLCP::waitForConnection(uint16_t timeout)
+{
+    uint8_t type;
+
+    mode = 1;
+    ns = 0;
+    nr = 0;
+
+    // Get CONNECT PDU
+    DMSG("wait for a CONNECT PDU\n");
+    do {
+        if (2 > link.read(headerBuf, headerBufLen)) {
+            return -1;
+        }
+
+        type = getPType(headerBuf);
+        if (PDU_CONNECT == type) {
+            break;
+        } else if (PDU_SYMM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return -2;
+            }
+        } else {
+            return -3;
+        }
+
+    } while (1);
+
+    // Put CC PDU
+    DMSG("put a CC(Connection Complete) PDU to response the CONNECT PDU\n");
+    ssap = getDSAP(headerBuf);
+    dsap = getSSAP(headerBuf);
+    headerBuf[0] = (dsap << 2) + ((PDU_CC >> 2) & 0x3);
+    headerBuf[1] = ((PDU_CC & 0x3) << 6) + ssap;
+    if (!link.write(headerBuf, 2)) {
+        return -2;
+    }
+
+    return 1;
+}
+
+int8_t LLCP::waitForDisconnection(uint16_t timeout)
+{
+    uint8_t type;
+
+    // Get DISC PDU
+    DMSG("wait for a DISC PDU\n");
+    do {
+        if (2 > link.read(headerBuf, headerBufLen)) {
+            return -1;
+        }
+
+        type = getPType(headerBuf);
+        if (PDU_DISC == type) {
+            break;
+        } else if (PDU_SYMM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return -2;
+            }
+        } else {
+            return -3;
+        }
+
+    } while (1);
+
+    // Put DM PDU
+    DMSG("put a DM(Disconnect Mode) PDU to response the DISC PDU\n");
+    // ssap = getDSAP(headerBuf);
+    // dsap = getSSAP(headerBuf);
+    headerBuf[0] = (dsap << 2) + (PDU_DM >> 2);
+    headerBuf[1] = ((PDU_DM & 0x3) << 6) + ssap;
+    if (!link.write(headerBuf, 2)) {
+        return -2;
+    }
+
+    return 1;
+}
+
+int8_t LLCP::connect(uint16_t timeout)
+{
+    uint8_t type;
+
+    mode = 0;
+    dsap = LLCP_DEFAULT_DSAP;
+    ssap = LLCP_DEFAULT_SSAP;
+    ns = 0;
+    nr = 0;
+
+    // try to get a SYMM PDU
+    if (2 > link.read(headerBuf, headerBufLen)) {
+        return -1;
+    }
+    type = getPType(headerBuf);
+    if (PDU_SYMM != type) {
+        return -1;
+    }
+
+    // put a CONNECT PDU
+    headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_CONNECT >> 2);
+    headerBuf[1] = ((PDU_CONNECT & 0x03) << 6) + LLCP_DEFAULT_SSAP;
+    uint8_t body[] = "  urn:nfc:sn:snep";
+    body[0] = 0x06;
+    body[1] = sizeof(body) - 2 - 1;
+    if (!link.write(headerBuf, 2, body, sizeof(body) - 1)) {
+        return -2;
+    }
+
+    // wait for a CC PDU
+    DMSG("wait for a CC PDU\n");
+    do {
+        if (2 > link.read(headerBuf, headerBufLen)) {
+            return -1;
+        }
+
+        type = getPType(headerBuf);
+        if (PDU_CC == type) {
+            break;
+        } else if (PDU_SYMM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return -2;
+            }
+        } else {
+            return -3;
+        }
+
+    } while (1);
+
+    return 1;
+}
+
+int8_t LLCP::disconnect(uint16_t timeout)
+{
+    uint8_t type;
+
+    // try to get a SYMM PDU
+    if (2 > link.read(headerBuf, headerBufLen)) {
+        return -1;
+    }
+    type = getPType(headerBuf);
+    if (PDU_SYMM != type) {
+        return -1;
+    }
+
+    // put a DISC PDU
+    headerBuf[0] = (LLCP_DEFAULT_DSAP << 2) + (PDU_DISC >> 2);
+    headerBuf[1] = ((PDU_DISC & 0x03) << 6) + LLCP_DEFAULT_SSAP;
+    if (!link.write(headerBuf, 2)) {
+        return -2;
+    }
+
+    // wait for a DM PDU
+    DMSG("wait for a DM PDU\n");
+    do {
+        if (2 > link.read(headerBuf, headerBufLen)) {
+            return -1;
+        }
+
+        type = getPType(headerBuf);
+        if (PDU_CC == type) {
+            break;
+        } else if (PDU_DM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return -2;
+            }
+        } else {
+            return -3;
+        }
+
+    } while (1);
+
+    return 1;
+}
+
+bool LLCP::write(const uint8_t *header, uint8_t hlen, const uint8_t *body, uint8_t blen)
+{
+    uint8_t type;
+    uint8_t buf[3];
+
+    if (mode) {
+        // Get a SYMM PDU
+        if (2 != link.read(buf, sizeof(buf))) {
+            return false;
+        }
+    }
+
+    if (headerBufLen < (hlen + 3)) {
+        return false;
+    }
+
+    for (int8_t i = hlen - 1; i >= 0; i--) {
+        headerBuf[i + 3] = header[i];
+    }
+
+    headerBuf[0] = (dsap << 2) + (PDU_I >> 2);
+    headerBuf[1] = ((PDU_I & 0x3) << 6) + ssap;
+    headerBuf[2] = (ns << 4) + nr;
+    if (!link.write(headerBuf, 3 + hlen, body, blen)) {
+        return false;
+    }
+
+    ns++;
+
+    // Get a RR PDU
+    int16_t status;
+    do {
+        status = link.read(headerBuf, headerBufLen);
+        if (2 > status) {
+            return false;
+        }
+
+        type = getPType(headerBuf);
+        if (PDU_RR == type) {
+            break;
+        } else if (PDU_SYMM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return false;
+            }
+        } else {
+            return false;
+        }
+    } while (1);
+
+    if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+        return false;
+    }
+
+    return true;
+}
+
+int16_t LLCP::read(uint8_t *buf, uint8_t length)
+{
+    uint8_t type;
+    int16_t status;
+
+    // Get INFO PDU
+    do {
+        status = link.read(buf, length);
+        if (2 > status) {
+            return -1;
+        }
+
+        type = getPType(buf);
+        if (PDU_I == type) {
+            break;
+        } else if (PDU_SYMM == type) {
+            if (!link.write(SYMM_PDU, sizeof(SYMM_PDU))) {
+                return -2;
+            }
+        } else {
+            return -3;
+        }
+
+    } while (1);
+
+    uint8_t len = status - 3;
+    ssap = getDSAP(buf);
+    dsap = getSSAP(buf);
+
+    headerBuf[0] = (dsap << 2) + (PDU_RR >> 2);
+    headerBuf[1] = ((PDU_RR & 0x3) << 6) + ssap;
+    headerBuf[2] = (buf[2] >> 4) + 1;
+    if (!link.write(headerBuf, 3)) {
+        return -2;
+    }
+
+    for (uint8_t i = 0; i < len; i++) {
+        buf[i] = buf[i + 3];
+    }
+
+    nr++;
+
+    return len;
+}