FTDI driver: Filter status bytes

Filter status bytes at the beginning of every USB package received from
FTDI serial adapters

UsbSerialLibrary/src/com/hoho/android/usbserial/driver/FtdiSerialDriver.java

@@ -166,6 +166,32 @@ private static enum DeviceType {
      */
     private static final boolean ENABLE_ASYNC_READS = false;
 
+    /**
+     * Filter FTDI status bytes from buffer
+     * @param src The source buffer (which contains status bytes)
+     * @param dest The destination buffer to write the status bytes into (can be src)
+     * @param totalBytesRead Number of bytes read to src
+     * @param maxPacketSize The USB endpoint max packet size
+     * @return The number of payload bytes
+     */
+    private final int filterStatusBytes(byte[] src, byte[] dest, int totalBytesRead, int maxPacketSize) {
+        final int packetsCount = totalBytesRead / maxPacketSize + 1;
+        for (int packetIdx = 0; packetIdx < packetsCount; ++packetIdx) {
+            final int count = (packetIdx == (packetsCount - 1))
+                    ? (totalBytesRead % maxPacketSize) - MODEM_STATUS_HEADER_LENGTH
+                    : maxPacketSize - MODEM_STATUS_HEADER_LENGTH;
+            if (count > 0) {
+                System.arraycopy(src,
+                        packetIdx * maxPacketSize + MODEM_STATUS_HEADER_LENGTH,
+                        dest,
+                        packetIdx * (maxPacketSize - MODEM_STATUS_HEADER_LENGTH),
+                        count);
+            }
+        }
+
+      return totalBytesRead - (packetsCount * 2);
+    }
+
     /**
      * Constructor.
      *
@@ -253,17 +279,13 @@ public int read(byte[] dest, int timeoutMillis) throws IOException {
                 final int readAmt = Math.min(dest.length, mReadBuffer.length);
                 totalBytesRead = mConnection.bulkTransfer(endpoint, mReadBuffer,
                         readAmt, timeoutMillis);
-            }
-
-            if (totalBytesRead < MODEM_STATUS_HEADER_LENGTH) {
-                throw new IOException("Expected at least " + MODEM_STATUS_HEADER_LENGTH + " bytes");
-            }
 
-            final int payloadBytesRead = totalBytesRead - MODEM_STATUS_HEADER_LENGTH;
-            if (payloadBytesRead > 0) {
-                System.arraycopy(mReadBuffer, MODEM_STATUS_HEADER_LENGTH, dest, 0, payloadBytesRead);
+                if (totalBytesRead < MODEM_STATUS_HEADER_LENGTH) {
+                    throw new IOException("Expected at least " + MODEM_STATUS_HEADER_LENGTH + " bytes");
+                }
+
+                return filterStatusBytes(mReadBuffer, dest, totalBytesRead, endpoint.getMaxPacketSize());
             }
-            return payloadBytesRead;
         }
     }