b35d5a8f4fdc85dbd66f85f7f5c9158c5278b00a
[SCSI2SD-V6.git] / 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