src/firmware/hidpacket.c

   1 //      Copyright (C) 2014 Michael McMaster <michael@codesrc.com>
   2 //
   3 //      This file is part of SCSI2SD.
   4 //
   5 //      SCSI2SD is free software: you can redistribute it and/or modify
   6 //      it under the terms of the GNU General Public License as published by
   7 //      the Free Software Foundation, either version 3 of the License, or
   8 //      (at your option) any later version.
   9 //
  10 //      SCSI2SD is distributed in the hope that it will be useful,
  11 //      but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 //      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13 //      GNU General Public License for more details.
  14 //
  15 //      You should have received a copy of the GNU General Public License
  16 //      along with SCSI2SD.  If not, see <http://www.gnu.org/licenses/>.
  17
  18 #include "hidpacket.h"
  19 #include <string.h>
  20
  21 enum STATE { IDLE, PARTIAL, COMPLETE };
  22 typedef struct
  23 {
  24         uint8_t buffer[HIDPACKET_MAX_LEN];
  25
  26         int state;
  27         uint8_t chunk;
  28         size_t offset; // Current offset into buffer.
  29 } HIDPacketState;
  30 static HIDPacketState rx  __attribute__((aligned(USBHID_LEN))) = {{}, IDLE, 0, 0};
  31 static HIDPacketState tx  __attribute__((aligned(USBHID_LEN))) = {{}, IDLE, 0, 0};
  32
  33 static void
  34 rxReset()
  35 {
  36         rx.state = IDLE;
  37         rx.chunk = 0;
  38         rx.offset = 0;
  39 }
  40
  41 static void
  42 txReset()
  43 {
  44         tx.state = IDLE;
  45         tx.chunk = 0;
  46         tx.offset = 0;
  47 }
  48
  49 void hidPacket_recv(const uint8_t* bytes, size_t len)
  50 {
  51         if (len < 2)
  52         {
  53                 // Invalid. We need at least a chunk number and payload length.
  54                 rxReset();
  55                 return;
  56         }
  57
  58         uint8_t chunk = bytes[0] & 0x7F;
  59         int final = bytes[0] & 0x80;
  60         uint8_t payloadLen = bytes[1];
  61         if ((payloadLen > (len - 2)) || // short packet
  62                 (payloadLen + rx.offset > sizeof(rx.buffer))) // buffer overflow
  63         {
  64                 rxReset();
  65                 return;
  66         }
  67
  68         if (chunk == 0)
  69         {
  70                 // Initial chunk
  71                 rxReset();
  72                 memcpy(rx.buffer, bytes + 2, payloadLen);
  73                 rx.offset = payloadLen;
  74                 rx.state = PARTIAL;
  75         }
  76         else if ((rx.state == PARTIAL) && (chunk == rx.chunk + 1))
  77         {
  78                 memcpy(rx.buffer + rx.offset, bytes + 2, payloadLen);
  79                 rx.offset += payloadLen;
  80                 rx.chunk++;
  81         }
  82         else if (chunk == rx.chunk)
  83         {
  84                 // duplicated packet. ignore.
  85         }
  86         else
  87         {
  88                 // invalid. Maybe we missed some data.
  89                 rxReset();
  90         }
  91
  92         if ((rx.state == PARTIAL) && final)
  93         {
  94                 rx.state = COMPLETE;
  95         }
  96 }
  97
  98 const uint8_t*
  99 hidPacket_getPacket(size_t* len)
 100 {
 101         if (rx.state == COMPLETE)
 102         {
 103                 *len = rx.offset;
 104                 rxReset();
 105                 return rx.buffer;
 106         }
 107         else
 108         {
 109                 *len = 0;
 110                 return NULL;
 111         }
 112 }
 113
 114 void hidPacket_send(const uint8_t* bytes, size_t len)
 115 {
 116         if (len <= sizeof(tx.buffer))
 117         {
 118                 tx.state = PARTIAL;
 119                 tx.chunk = 0;
 120                 tx.offset = len;
 121                 memcpy(tx.buffer, bytes, len);
 122         }
 123         else
 124         {
 125                 txReset();
 126         }
 127 }
 128
 129 const uint8_t*
 130 hidPacket_getHIDBytes(uint8_t* hidBuffer)
 131 {
 132         if ((tx.state != PARTIAL) || (tx.offset <= 0))
 133         {
 134                 return NULL;
 135         }
 136
 137         hidBuffer[0] = tx.chunk;
 138         tx.chunk++;
 139         uint8_t payload;
 140         if (tx.offset <= USBHID_LEN - 2)
 141         {
 142                 hidBuffer[0] = hidBuffer[0] | 0x80;
 143                 payload = tx.offset;
 144                 tx.state = IDLE;
 145                 memset(hidBuffer + 2, 0, USBHID_LEN - 2);
 146         }
 147         else
 148         {
 149                 payload = USBHID_LEN - 2;
 150         }
 151
 152         tx.offset -= payload;
 153         hidBuffer[1] = payload;
 154         memcpy(hidBuffer + 2, tx.buffer, payload);
 155         memmove(tx.buffer, tx.buffer + payload, sizeof(tx.buffer) - payload);
 156
 157         return hidBuffer;
 158 }
 159
 160