forked from UWARG/RC-Car-Bootstrap
/
net_inbound.c
executable file
·152 lines (137 loc) · 4.37 KB
/
net_inbound.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
/**
* net_inbound.c
*/
#include "net.h"
#include <stdlib.h>
#include "UART1.h"
#define START_DELIMITER 0x7E
typedef enum {
EMPTY = 0, // Can write new data here
BUSY = 1, // Data being written here
READY = 2 // Data ready
} PacketStatus;
struct command* inBuffer[INBOUND_QUEUE_SIZE];
int inbuff_start = 0;
int inbuff_end = 0;
char rawPackets[RAW_PACKET_BUFFER_SIZE][128];
PacketStatus rawPacketStatus[RAW_PACKET_BUFFER_SIZE];
static int packetPos = 0;
static int payloadPos = 0;
static int payloadLength[RAW_PACKET_BUFFER_SIZE];
// Clean up the command
void destroyCommand( struct command* cmd ) {
free( cmd );
cmd = 0;
}
// Get the
struct command* popCommand() {
struct command* cmd = inBuffer[inbuff_start];
if ( ! cmd ) return 0;
inBuffer[inbuff_start] = 0;
inbuff_start = ( inbuff_start + 1 ) % INBOUND_QUEUE_SIZE;
return cmd;
}
int pushCommand(struct command* cmd) {
if ( inbuff_end == ( inbuff_start - 1 + INBOUND_QUEUE_SIZE ) % INBOUND_QUEUE_SIZE ) {
return 0; // If no room for commands, fail
}
inBuffer[inbuff_end] = cmd; // Insert cmd at end of queue
inbuff_end = ( inbuff_end + 1 ) % INBOUND_QUEUE_SIZE; // increment handling wrap
return 1;
}
struct command* createCommand( char* rawPacket ) {
struct command* cmd = malloc(sizeof(struct command));
if ( ! cmd ) { // Malloc failed ?
return 0;
}
int k;
char temp[128];
for (k = 0; k < 128; k++) temp[k] = rawPacket[k];
cmd->data_length = rawPacket[2];
cmd->cmd = rawPacket[15];
int i;
int j = 0;
for ( i = 16; i < 15 + cmd->data_length - 12; i++ ) { // Received packet payload starts at 15 and has 12 bytes before len
cmd->data[j++] = rawPacket[i];
}
cmd->data[j] = '\0'; // Null terminate the string so can use SendUart
// if ( cmd->data[0] == 0 ) {
//// UART1_SendString("ERROR");
// int zero = 1;
// zero += 1;
// return 0;
// }
return cmd;
}
// Return 1 if packet is valid
int checkPacket(char* rawPacket) {
unsigned int i = 2;
char packetLength = rawPacket[i++];
char checksum = 0;
char packetChecksum = 0;
for (i = 3; i < packetLength + 3; i++){
checksum += rawPacket[i];
}
packetChecksum = 0xFF - rawPacket[i];
if (checksum == packetChecksum){
return 1; // TODO: checksums are for suckers, because fuck you, thats why
}
else{
return 0;
}
}
void inboundBufferMaintenance(void) {
int i;
for ( i = 0; i < RAW_PACKET_BUFFER_SIZE; i++ ) {
if ( rawPacketStatus[i] == READY && checkPacket(rawPackets[i]) ) {
struct command* cmd = createCommand( rawPackets[i] );
if ( cmd ) { // create command was successful ?
pushCommand( cmd ); // queue it up
rawPacketStatus[i] = EMPTY; // buffer is now good for writing another packet
}
}
}
if ( rawPacketStatus[0] == EMPTY ) {
rawPacketStatus[0] = BUSY;
}
}
void __attribute__((__interrupt__, no_auto_psv)) _U2RXInterrupt(void) {
unsigned char data = U2RXREG;
if ( rawPacketStatus[packetPos] != BUSY ) { // no buffer available to write
packetPos = ( packetPos + 1 ) % RAW_PACKET_BUFFER_SIZE;
IFS1bits.U2RXIF = 0;
return;
}
switch ( payloadPos ) {
case 0:
if ( data != START_DELIMITER ) {
// if (U2STAbits.OERR == 1)
// U2STAbits.OERR = 0;
IFS1bits.U2RXIF = 0;
return;
}
break;
case 1:
if ( data != 0 ) {
payloadPos = 0;
IFS1bits.U2RXIF = 0;
return; // packet length < 100 bytes, so msb == 0
}
break;
case 2:
payloadLength[packetPos] = data;
break;
default: // Normally, don't do anything special
break;
}
rawPackets[packetPos][payloadPos++] = data;
if ( payloadPos && payloadPos == payloadLength[packetPos] + 3 + 1) { // at end of packet
rawPacketStatus[packetPos] = READY;
payloadPos = 0;
packetPos = ( packetPos + 1 ) % RAW_PACKET_BUFFER_SIZE;
if ( rawPacketStatus[packetPos] == EMPTY ) {
rawPacketStatus[packetPos] = BUSY;
}
}
IFS1bits.U2RXIF = 0;
}