Synch transfer fix
[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 const uint8_t*
115 hidPacket_peekPacket(size_t* len)
116 {
117         if (rx.state == COMPLETE)
118         {
119                 *len = rx.offset;
120                 return rx.buffer;
121         }
122         else
123         {
124                 *len = 0;
125                 return NULL;
126         }
127 }
128
129 void hidPacket_send(const uint8_t* bytes, size_t len)
130 {
131         if (len <= sizeof(tx.buffer))
132         {
133                 tx.state = PARTIAL;
134                 tx.chunk = 0;
135                 tx.offset = len;
136                 memcpy(tx.buffer, bytes, len);
137         }
138         else
139         {
140                 txReset();
141         }
142 }
143
144 const uint8_t*
145 hidPacket_getHIDBytes(uint8_t* hidBuffer)
146 {
147         if ((tx.state != PARTIAL) || (tx.offset <= 0))
148         {
149                 return NULL;
150         }
151
152         hidBuffer[0] = tx.chunk;
153         tx.chunk++;
154         uint8_t payload;
155         if (tx.offset <= USBHID_LEN - 2)
156         {
157                 hidBuffer[0] = hidBuffer[0] | 0x80;
158                 payload = tx.offset;
159                 tx.state = IDLE;
160                 memset(hidBuffer + 2, 0, USBHID_LEN - 2);
161         }
162         else
163         {
164                 payload = USBHID_LEN - 2;
165         }
166
167         tx.offset -= payload;
168         hidBuffer[1] = payload;
169         memcpy(hidBuffer + 2, tx.buffer, payload);
170         memmove(tx.buffer, tx.buffer + payload, sizeof(tx.buffer) - payload);
171
172         return hidBuffer;
173 }
174
175