-
Notifications
You must be signed in to change notification settings - Fork 2
/
core.c
268 lines (224 loc) · 9.09 KB
/
core.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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <stdio.h>
#include "base64.h"
#include "logger.h"
#include "core.h"
#include "server.h"
#include "util.h"
/**
* Manages subscriptions and forwarding CSI messages to subscribers
*/
struct subscription* subscriptions[100];
int subscriptionsLength = 0;
char buffer[20000];
// ID of the message, incremented with each received CSI info and included in each notification
int messageId;
bool atherosStdoutDumpEnabled = false;
/**
* Packages csi matrix and status into a buffer to be sent to a subscriber
* @param buffer the buffer to write to
* @param csi_status the csi status
* @param csi_matrix the csi matrix
* @returns the length of the packaged data
*/
int ath_packageCSIInfoMessage(char* buffer, ath_csi_struct* csi_status, ATH_COMPLEX csi_matrix[3][3][114]) {
int index = 0;
putByte(buffer, &index, TYPE_ATH_CSI_INFO);
putInt(buffer, &index, messageId);
putLong(buffer, &index, csi_status->tstamp);
putShort(buffer, &index, csi_status->channel);
putByte(buffer, &index, csi_status->chanBW);
putByte(buffer, &index, csi_status->rate);
putByte(buffer, &index, csi_status->nr);
putByte(buffer, &index, csi_status->nc);
putByte(buffer, &index, csi_status->num_tones);
putByte(buffer, &index, csi_status->noise);
putByte(buffer, &index, csi_status->phyerr);
putByte(buffer, &index, csi_status->rssi);
putByte(buffer, &index, csi_status->rssi_0);
putByte(buffer, &index, csi_status->rssi_1);
putByte(buffer, &index, csi_status->rssi_2);
putShort(buffer, &index, csi_status->payload_len);
putShort(buffer, &index, csi_status->csi_len);
putShort(buffer, &index, csi_status->buf_len);
if(csi_status->csi_len > 0) {
for(int i1 = 0;i1 < 3;i1++) {
for(int i2 = 0;i2 < 3;i2++) {
// for(int i3 = 0;i3 < 114;i3++) {
for(int i3 = 0;i3 < csi_status->num_tones;i3++) {
putInt(buffer, &index, csi_matrix[i1][i2][i3].real);
putInt(buffer, &index, csi_matrix[i1][i2][i3].imag);
}
}
}
} else {
memset((buffer + index), 0, sizeof(ATH_COMPLEX) * 3 * 3 * csi_status->num_tones);
index += sizeof(ATH_COMPLEX) * 3 * 3 * csi_status->num_tones;
}
return index;
}
void ath_dumpCSIInfoMessageToStdout(char* buffer, ath_csi_struct* csi_status, ATH_COMPLEX csi_matrix[3][3][114]) {
printf("<athCSI>");
int len = ath_packageCSIInfoMessage(buffer, csi_status, csi_matrix);
// for(int i = 0;i < len;i++) {
// // printf("%x", buffer[i]);
// }
size_t encodedLength = 0;
char* base64EncodedBuffer = base64_encode((unsigned char*)buffer, len, &encodedLength); // null terminated, malloc'd buffer
printf("%s", base64EncodedBuffer);
free(base64EncodedBuffer);
printf("</athCSI>\n");
fflush(stdout);
}
/**
* Packages csi matrix and status into a buffer to be sent to a subscriber
* @param buffer the buffer to write to
* @param notification the csi notification
* @returns the length of the packaged data
*/
int int_packageCSIInfoMessage(char* buffer, int_csi_notification* notification) {
int index = 0;
// send as longer type to prevent conversion to signed at receiver
uint16_t rssi_a = notification->rssi_a;
uint16_t rssi_b = notification->rssi_b;
uint16_t rssi_c = notification->rssi_c;
uint16_t noise = notification->noise;
uint16_t agc = notification->agc;
putByte(buffer, &index, TYPE_INT_CSI_INFO);
putInt(buffer, &index, messageId);
putInt(buffer, &index, notification->timestamp_low);
putShort(buffer, &index, notification->bfee_count);
putByte(buffer, &index, notification->Nrx);
putByte(buffer, &index, notification->Ntx);
putShort(buffer, &index, rssi_a);
putShort(buffer, &index, rssi_b);
putShort(buffer, &index, rssi_c);
putShort(buffer, &index, noise);
putShort(buffer, &index, agc);
putByte(buffer, &index, notification->antenna_sel);
putByte(buffer, &index, notification->perm[0]);
putByte(buffer, &index, notification->perm[1]);
putByte(buffer, &index, notification->perm[2]);
putShort(buffer, &index, notification->len);
putShort(buffer, &index, notification->fake_rate_n_flags);
// format: [tx][rx][carrier] !!!!! (permutation of rx ant <-> rf chains is already applied)
int csi_mat_entries = notification->Ntx * notification->Nrx * 30;
for(int i = 0;i < csi_mat_entries;i++) {
putDouble(buffer, &index, notification->csi_matrix[i].real);
putDouble(buffer, &index, notification->csi_matrix[i].imag);
}
return index;
}
/**
* Called on receiving CSI from the kernel, checks filters and forwards to subscribers
* @param data_buf the data from the kernel
* @param csi_status the csi status from the kernel
* @param csi_matrix the csi matrix from the kernel
*/
void ath_onCSI(unsigned char *data_buf, ath_csi_struct* csi_status, ATH_COMPLEX csi_matrix[3][3][114]) {
printf("Broadcasting to subscribers\n");
if(atherosStdoutDumpEnabled) {
ath_dumpCSIInfoMessageToStdout(buffer, csi_status, csi_matrix);
}
for(int i = 0;i < subscriptionsLength;i++) {
struct subscription* sub = subscriptions[i];
if(matchesFilter(csi_status, &(sub->options.filter_options))) {
int len = ath_packageCSIInfoMessage(buffer, csi_status, csi_matrix);
sendData(sub->address, sub->addressLength, buffer, len);
}
}
messageId++;
}
/**
* Called on receiving CSI from the kernel, does not yet filter and forwards to subscribers
* @param notification the notification constructed from the data received from the kernel
*/
void int_onCSI(int_csi_notification* notification) {
for(int i = 0;i < subscriptionsLength;i++) {
struct subscription* sub = subscriptions[i];
// TODO: check / add filter options
// if(matchesFilter(csi_status, &(sub->options.filter_options))) {
int len = int_packageCSIInfoMessage(buffer, notification);
sendData(sub->address, sub->addressLength, buffer, len);
// }
}
messageId++;
}
/**
* Adds a subscription for a client
* @param buf the received subscription message
* @param len length of the subscription message
* @param clientAddress the address of the subscriber to be added
* @param addressLength the length of the subscriber's address
*/
void subscribe(char* buf, int len, struct sockaddr_in* clientAddress, socklen_t addressLength) {
if(len != sizeof(struct subscription_options)) {
log(LEVEL_WARNING, "Subscription message has wrong length");
return;
}
unsubscribe(NULL, 0, clientAddress, addressLength);
struct subscription* subscription = malloc(sizeof(struct subscription));
memcpy(&(subscription->options), buf, sizeof(struct subscription_options));
subscription->address = malloc(sizeof(struct sockaddr_in));
memcpy(subscription->address, clientAddress, sizeof(struct sockaddr_in));
subscription->addressLength = addressLength;
subscriptions[subscriptionsLength] = subscription;
subscriptionsLength++;
log(LEVEL_INFO, "Subscription from %s", inet_ntoa(clientAddress->sin_addr));
char confirmation[] = {TYPE_CONFIRM_SUBSCRIPTION};
sendData(clientAddress, addressLength, confirmation, 1);
}
//TODO: maybe just allow one client?
/**
* Removes a subscriber
* @param buf the unsubscription message
* @param len the length of the unsubscription message
* @param clientAddress the address of the client
* @param addressLength the length of the client's address
*/
void unsubscribe(char* buf, int len, struct sockaddr_in* clientAddress, socklen_t addressLength) {
//char confirmation[] = {TYPE_CONFIRM_UNSUBSCRIPTION};
//sendData(clientAddress, addressLength, confirmation, 1);
int i = 0;
for(;i < subscriptionsLength;i++) {
struct sockaddr_in* otherAddr = subscriptions[i]->address;
if(clientAddress->sin_addr.s_addr == otherAddr->sin_addr.s_addr
&& clientAddress->sin_port == otherAddr->sin_port) {
goto found; // This the only goto I have ever and will ever use in my life (and it's probably one too much)
}
}
return;
found:
log(LEVEL_INFO, "Removing %s", inet_ntoa(clientAddress->sin_addr));
free(subscriptions[i]->address);
free(subscriptions[i]);
i++;
for(;i < subscriptionsLength;i++) {
subscriptions[i - 1] = subscriptions[i];
}
subscriptionsLength--;
}
/**
* Checks if the current csi info matches a client's subscription options, can be expanded
* @param csi_status
* @param options the client's filter options
* @returns 1 if matches client's filter, 0 else
*/
int matchesFilter(ath_csi_struct* csi_status, struct filter_options* options) {
if(options->payload_size != 0 && csi_status->payload_len != options->payload_size) {
return 0;
}
//built this way to accomodate more filters
return 1;
}
void setAtherosStdoutDumpEnabled(bool enabled) {
atherosStdoutDumpEnabled = enabled;
if(atherosStdoutDumpEnabled) {
log(LEVEL_INFO, "Dumping (atheros) csi data to stdout ");
}
}