Line data Source code
1 : #include <stdlib.h>
2 : #include <stdio.h>
3 : #include <time.h>
4 : #include <string.h>
5 : #include <unistd.h>
6 : #include <signal.h>
7 : #include <stdbool.h>
8 :
9 : #include "felix/felix_fid.h"
10 : #include "felix-star/client.h" // for toflx header
11 : #include "netio/netio.h"
12 : #include "config.h"
13 : #include "flx_api.h"
14 : #include "fifo.h"
15 : #include "log.h"
16 : #include "statistics.h"
17 : #include "bus.h"
18 : #include "felixtag.h"
19 : #include "regmap/regmap-symbol.h"
20 :
21 : #define FLX_DMA_WRAPAROUND 1
22 :
23 : struct netio_container {
24 : struct netio_context ctx;
25 : // unbuffered
26 : struct netio_listen_socket socket;
27 : // struct netio_buffer_table buffer_table;
28 :
29 : // buffered
30 : struct netio_buffered_socket_attr attr;
31 : struct netio_buffered_listen_socket buffered_socket;
32 : };
33 :
34 : struct toflx_application {
35 : struct config cfg;
36 : struct netio_container netio;
37 : struct flx flx;
38 : int flxcard_max_tlp; // caching to avoid requesting on each msg encoding and each dma transfer
39 : struct cmem_buffer dma_buffer;
40 : struct toflx_stats_data stats_data;
41 : struct felix_statistics stats_writer;
42 :
43 : uint64_t dma_softw_ptr; // Copy of the DMA 'software-controlled' pointer for updates and write-back
44 :
45 : struct elink_descr* elinks;
46 : unsigned num_elinks;
47 :
48 : int regmap;
49 : void (*transfer_message)(void* data, size_t len);
50 : };
51 :
52 : struct toflx_application app;
53 :
54 : // Forward declarations
55 : void on_connection_established(struct netio_recv_socket*);
56 : void on_msg_received(struct netio_recv_socket*, struct netio_buffer* buf, void* data, size_t len);
57 : void on_connection_closed(struct netio_recv_socket * sock);
58 : void on_buffered_connection_established(struct netio_buffered_recv_socket*);
59 : void on_buffered_msg_received(struct netio_buffered_recv_socket*, void* data, size_t len);
60 : // void on_buffered_msg_received_cdma(struct netio_buffered_recv_socket*, void* data, size_t len);
61 : void on_buffered_connection_closed(struct netio_buffered_recv_socket * sock);
62 : void on_stats(void* ptr);
63 : // Single-shot DMA functions
64 : void dma_single_shot_transfer(void* data, size_t len);
65 : void dma_single_shot_write(size_t size);
66 : // Circular DMA functions
67 : void dma_circular_transfer(void* data, size_t len);
68 : bool dma_circular_wait();
69 :
70 : // unbuffered
71 14 : void on_connection_closed(struct netio_recv_socket * sock) { //cppcheck-suppress [constParameter]
72 14 : LOG_INFO("Connection closed");
73 14 : }
74 :
75 : // Buffered
76 19 : void on_buffered_connection_closed(struct netio_buffered_recv_socket * sock) {
77 19 : LOG_INFO("Connection closed");
78 : //netio_buffered_remove_recv_socket(sock);
79 : //TODO close socket and reinit
80 19 : }
81 : // Callbacks
82 39 : void on_init(void* data)
83 : {
84 39 : if (app.cfg.unbuffered) {
85 16 : struct netio_unbuffered_socket_attr attr = {app.cfg.netio_pages, app.cfg.netio_pagesize};
86 16 : netio_init_listen_socket(&app.netio.socket, &app.netio.ctx, &attr);
87 16 : netio_listen(&app.netio.socket, app.cfg.ip, app.cfg.port);
88 : } else {
89 23 : app.netio.attr.num_pages = app.cfg.netio_pages;
90 23 : app.netio.attr.pagesize = app.cfg.netio_pagesize;
91 23 : app.netio.attr.watermark = app.cfg.netio_watermark;
92 23 : netio_buffered_listen_socket_init(&app.netio.buffered_socket, &app.netio.ctx, &app.netio.attr);
93 23 : netio_buffered_listen(&app.netio.buffered_socket, app.cfg.ip, app.cfg.port);
94 : }
95 :
96 39 : if (app.cfg.unbuffered) {
97 16 : LOG_INFO("Using unbuffered mode");
98 16 : app.netio.socket.cb_connection_established = on_connection_established;
99 16 : app.netio.socket.cb_msg_received = on_msg_received;
100 16 : app.netio.socket.cb_connection_closed = on_connection_closed;
101 : } else {
102 23 : LOG_INFO("Using buffered mode");
103 23 : app.netio.buffered_socket.cb_connection_established = on_buffered_connection_established;
104 23 : app.netio.buffered_socket.cb_msg_received = on_buffered_msg_received;
105 23 : app.netio.buffered_socket.cb_connection_closed = on_buffered_connection_closed;
106 : }
107 :
108 39 : if (app.cfg.statistics_fifo || app.cfg.statistics_stdout) {
109 2 : struct timespec now;
110 2 : clock_gettime(CLOCK_MONOTONIC_RAW, &now);
111 2 : reset_toflx_stats_data(&app.stats_data, &now);
112 2 : app.stats_writer.timer.cb = on_stats;
113 2 : netio_timer_start_ms(&app.stats_writer.timer, app.cfg.statistics_period);
114 : }
115 39 : }
116 :
117 : // Unbuffered
118 14 : void on_connection_established(struct netio_recv_socket* socket)
119 : {
120 14 : LOG_INFO("Connection established");
121 14 : }
122 :
123 : // Unbuffered
124 174 : void on_msg_received(struct netio_recv_socket* socket, struct netio_buffer* buf, void* data, size_t len)
125 : {
126 174 : LOG_DBG("Msg received");
127 174 : on_buffered_msg_received(NULL, buf->data, len);
128 174 : netio_post_recv(socket, buf);
129 174 : }
130 :
131 21 : void on_buffered_connection_established(struct netio_buffered_recv_socket* socket)
132 : {
133 21 : LOG_INFO("Connection established");
134 21 : }
135 :
136 315 : void on_buffered_msg_received(struct netio_buffered_recv_socket* socket, void* data, size_t len)
137 : {
138 315 : LOG_DBG("Msg received len=%d", len);
139 315 : app.stats_data.messages_received += 1;
140 315 : app.stats_data.bytes_received += len;
141 315 : app.transfer_message(data, len);
142 315 : }
143 :
144 313 : void dma_single_shot_transfer(void* data, size_t len)
145 : {
146 313 : size_t pos = 0;
147 313 : size_t dma_pos = 0;
148 :
149 : // The following assumes all 'data' will fit in the available size of app.dma_buffer.size
150 921 : while (pos < len) {
151 :
152 608 : if (len - pos < sizeof(struct to_flx_hdr)) {
153 0 : LOG_ERR("Not enough data to decode a ToFlxHdr: pos= %ld len= %ld\n", pos, len);
154 0 : return;
155 : }
156 :
157 608 : struct to_flx_hdr *hdr = (struct to_flx_hdr*) ((uint8_t*) data + pos);
158 608 : pos += sizeof(struct to_flx_hdr); // Update index
159 :
160 608 : if (hdr->length == 0) {
161 0 : LOG_ERR("Invalid chunk length: 0, pos= %ld len= %ld\n", pos, len);
162 0 : return;
163 : }
164 :
165 608 : LOG_DBG("Header len=%d", hdr->length);
166 608 : if (len - pos < hdr->length) {
167 0 : LOG_ERR("Not enough data to decode chunk content: pos= %ld len= %ld msg_len= %d\n",
168 : pos, len, hdr->length);
169 0 : return;
170 : }
171 :
172 608 : flx_encode_toflx_msg( (uint8_t*) data + pos,
173 : hdr->length,
174 608 : (char*) app.dma_buffer.vaddr,
175 : app.dma_buffer.size, // Size of the output DMA buffer
176 : &dma_pos, // Index into the output DMA buffer (where to continue filling)
177 : hdr->elink );
178 :
179 : // Message was processed, update index
180 608 : pos += hdr->length;
181 :
182 : // Update stats
183 608 : app.stats_data.toflx_received += 1;
184 : }
185 :
186 : // Transfer data in the buffer
187 313 : if (dma_pos != 0) {
188 313 : dma_single_shot_write(dma_pos);
189 : }
190 : }
191 :
192 2 : void dma_circular_transfer(void* data, size_t len)
193 : {
194 2 : size_t pos = 0;
195 2 : size_t dma_pos = app.dma_softw_ptr - app.dma_buffer.paddr;
196 :
197 2 : if(len > app.dma_buffer.size){
198 0 : LOG_ERR("Message larger than the DMA buffer");
199 0 : return;
200 : }
201 :
202 : // Check and wait until DMA transfer is not in progress or almost done, with timeout
203 2 : if( dma_circular_wait() ) {
204 : return; // Timeout
205 : }
206 :
207 : // The following assumes all 'data' will fit in the available size of app.dma_buffer.size
208 4 : while (pos < len) {
209 :
210 2 : if (len - pos < sizeof(struct to_flx_hdr)) {
211 0 : LOG_ERR("not enough data to decode a ToFlxHdr: pos= %ld len= %ld\n", pos, len);
212 0 : return;
213 : }
214 :
215 2 : struct to_flx_hdr *hdr = (struct to_flx_hdr*) ((uint8_t*) data + pos);
216 2 : pos += sizeof(struct to_flx_hdr); // Update index
217 :
218 2 : if (hdr->length == 0) {
219 0 : LOG_ERR("Invalid msg length: 0, pos= %ld len= %ld\n", pos, len);
220 0 : return;
221 : }
222 :
223 2 : LOG_DBG("Header len=%d", hdr->length);
224 2 : if (len - pos < hdr->length) {
225 0 : LOG_ERR("Not enough data to decode message content: pos= %ld len= %ld msg_len= %d\n", pos, len, hdr->length);
226 0 : return;
227 : }
228 :
229 2 : size_t nbytes;
230 4 : nbytes = flx_encode_toflx_msg( (uint8_t*) data + pos,
231 : hdr->length,
232 2 : (char*) app.dma_buffer.vaddr,
233 : app.dma_buffer.size, // Size of the output DMA buffer
234 : &dma_pos, // Index into the output DMA buffer (where to continue filling)
235 : hdr->elink );
236 :
237 : // Update the physical address value of the software pointer
238 2 : app.dma_softw_ptr += nbytes;
239 2 : if( app.dma_softw_ptr >= app.dma_buffer.paddr + app.dma_buffer.size )
240 : // Wrap-around
241 0 : app.dma_softw_ptr -= app.dma_buffer.size;
242 :
243 : // Message was processed, update index
244 2 : pos += hdr->length;
245 :
246 : // Update stats
247 2 : app.stats_data.toflx_received += 1;
248 : }
249 :
250 : // Update the sw_pointer in the DMA controller, which will (re)start/continue the DMA
251 2 : flx_dma_set_ptr(&app.flx, &app.dma_buffer, app.dma_softw_ptr);
252 : }
253 :
254 313 : void dma_single_shot_write(size_t size)
255 : {
256 313 : uint64_t transfer_ptr = app.dma_buffer.paddr;
257 313 : int const transfer_size = size;
258 313 : int wraparound = 0;
259 313 : LOG_DBG("flx_dma_from_host: dmaid = %d ptr = %p, size %d, wraparound %d", app.flx.dmaid, transfer_ptr, transfer_size, wraparound);
260 313 : flx_dma_from_host(&app.flx, &app.dma_buffer, transfer_ptr, transfer_size, wraparound);
261 :
262 313 : LOG_DBG("Waiting for single-shot DMA transfer to finish...");
263 : //#define USE_DMA_WAIT
264 : #ifdef USE_DMA_WAIT
265 : flx_dma_wait(&app.flx, &app.dma_buffer); // This is a busy-wait operation...
266 : #else
267 : // Poll for DMA finished
268 313 : bool timeout = false;
269 313 : uint64_t t_total = 0;
270 313 : while( flx_dma_enabled(&app.flx, &app.dma_buffer) ) {
271 0 : if( t_total > 1000000 ) {
272 : timeout = true;
273 : break;
274 : }
275 : // Wait: roughly the time it would take to transfer the data
276 : // into an 8-bit 8b10b E-link @ 320Mb/s (32 bytes/s);
277 0 : usleep( size/32 );
278 0 : t_total += size/32;
279 : }
280 313 : if( timeout ) {
281 0 : LOG_ERR("Single-shot DMA transfer to FLX device %d timed out", app.cfg.device);
282 : }
283 : #endif //USE_DMA_WAIT
284 313 : LOG_DBG("Single-shot DMA transfer finished");
285 313 : }
286 :
287 2 : bool dma_circular_wait()
288 : {
289 : // Check for DMA in-progress and wait if it is, so we need a time-out!
290 : // (although in fact we could continue filling the buffer until (almost) full)
291 2 : uint64_t t_total = 0;
292 2 : bool timeout = false;
293 2 : bool dma_in_progress = true;
294 4 : while( dma_in_progress ) {
295 :
296 2 : uint64_t curr_addr = flx_dma_get_current_address(&app.flx, &app.dma_buffer);
297 :
298 : // This condition should be sufficient:
299 2 : dma_in_progress = (curr_addr != app.dma_softw_ptr);
300 :
301 : // (Good choice, 1 sec? same as when FlxCard::dma_wait() is called)
302 2 : if( t_total > 1000000 ) {
303 0 : timeout = true;
304 0 : dma_in_progress = false;
305 : }
306 :
307 2 : if( dma_in_progress ) {
308 : // Wait: roughly the time it would take to transfer the remaining size
309 : // into an 8-bit 8b10b E-link @ 320Mb/s (32 bytes/s);
310 : // it doesn't make sense to check sooner
311 0 : uint64_t t; // In microseconds
312 0 : if( curr_addr > app.dma_softw_ptr )
313 0 : t = (curr_addr - app.dma_softw_ptr)/32;
314 : else
315 0 : t = ((app.dma_buffer.size + app.dma_softw_ptr) - curr_addr)/32;
316 0 : if( t > 1 ) { // No wait for 32 bytes or less
317 0 : usleep( t );
318 0 : t_total += t;
319 : }
320 : else {
321 : // No reason to wait any longer
322 : dma_in_progress = false;
323 : }
324 : }
325 : }
326 2 : if( timeout ) {
327 0 : LOG_ERR("Circular DMA transfer to FLX device %d timed out", app.cfg.device);
328 : }
329 2 : return timeout;
330 : }
331 :
332 : // Stats and debug
333 8 : void on_stats(void* ptr)
334 : {
335 8 : struct timespec current_ts;
336 8 : clock_gettime(CLOCK_MONOTONIC_RAW, ¤t_ts);
337 :
338 8 : if (app.cfg.statistics_stdout) {
339 8 : double const last_duration_sec = current_ts.tv_sec - app.stats_data.last_ts.tv_sec + 1e-9 * (current_ts.tv_nsec - app.stats_data.last_ts.tv_nsec);
340 8 : printf("data rate: %.2f MB/s message rate: %.2f kHz dma transfers: %lu toflx messages: %lu\n",
341 8 : app.stats_data.bytes_received/1024./1024./last_duration_sec,
342 8 : app.stats_data.messages_received/1000./last_duration_sec,
343 : app.stats_data.dma_transfers, app.stats_data.toflx_received);
344 : }
345 :
346 8 : if (app.cfg.statistics_fifo) {
347 8 : statistics_write_toflx(&app.stats_writer, &app.stats_data, ¤t_ts);
348 : }
349 :
350 8 : reset_toflx_stats_data(&app.stats_data, ¤t_ts);
351 8 : }
352 :
353 : // void dump_buffer(unsigned long vaddr, int size)
354 : // {
355 : // LOG_DBG("Dumping block ");
356 : // uint8_t *buf = (uint8_t *)vaddr;
357 : // int i;
358 : // for(i = 0; i < size; i++)
359 : // {
360 : // if (i % 32 == 0) {
361 : // printf("\n0x%016lx : ", i + vaddr);
362 : // }
363 : // printf("%02x ", *buf++);
364 : // }
365 : // printf("\n");
366 : // }
367 :
368 : // Exit handler
369 : static void
370 39 : on_quit()
371 : {
372 39 : LOG_DBG("Exit handler");
373 39 : log_flush();
374 39 : netio_terminate(&app.netio.ctx.evloop);
375 39 : }
376 :
377 : static void
378 39 : sigterm_handler (int sig)
379 : {
380 39 : LOG_INFO("Caught signal %d, cleaning up", sig);
381 39 : on_quit();
382 39 : }
383 :
384 39 : int main(int argc, char** argv)
385 : {
386 39 : cli_parse(argc, argv, &app.cfg);
387 :
388 39 : struct sigaction action;
389 39 : memset (&action, 0, sizeof(action));
390 39 : action.sa_handler = sigterm_handler;
391 39 : if (sigaction(SIGINT, &action, 0)) {
392 0 : perror ("sigaction");
393 0 : return 1;
394 : }
395 :
396 39 : netio_tag_t log_fid = get_fid(app.cfg.co, ERROR_LINK, 0, app.cfg.vid, 1, 1); // sid = 0; is_to_flx = 1, is_virtual = 1
397 39 : log_init(app.cfg.verbose, app.cfg.error_fifo, log_fid, app.cfg.appname, app.cfg.device);
398 :
399 39 : if (app.cfg.statistics_fifo) {
400 2 : netio_tag_t stats_fid = get_fid(app.cfg.co, STATS_LINK, 0, app.cfg.vid, 1, 1); // sid = 0; is_to_flx = 1, is_virtual = 1
401 2 : statistics_init(&app.stats_writer, app.cfg.statistics_fifo, stats_fid, &app.flx, &app.cfg, &app.dma_buffer);
402 : }
403 :
404 : // Determine toflx dmaid
405 39 : app.flx.dmaid = (app.cfg.dmaid != -1) ? app.cfg.dmaid : flx_get_toflx_dmaid(&app.cfg);
406 39 : LOG_INFO("Using DMA %d", app.flx.dmaid);
407 39 : app.cfg.lock_mask = 0;
408 : // For backward compatibility
409 39 : if (app.cfg.dmaid == 1) {
410 2 : app.cfg.lock_mask = LOCK_DMA1;
411 : }
412 : // Real lock
413 39 : app.cfg.lock_mask |= LOCK_DMA(app.cfg.dmaid);
414 :
415 39 : int rv = flx_init_card(&app.flx, &app.cfg);
416 39 : if (rv) {
417 0 : LOG_ERR("Could not open (virtual) device %d, lock_mask %d: error_code %d\n", app.cfg.device, LOCK_NONE, rv);
418 0 : return 1;
419 : }
420 :
421 39 : app.flxcard_max_tlp = 32; //flx_dma_max_tlp_bytes(&app.flx);
422 39 : LOG_INFO("Using max_tlp %d, while device has %d", app.flxcard_max_tlp, flx_dma_max_tlp_bytes(&app.flx));
423 39 : app.regmap = flx_get_regmap_version(&app.flx);
424 : #if REGMAP_VERSION < 0x0500
425 20 : if ((app.regmap < 0x0400) || (app.regmap >= 0x0500)) {
426 : #else
427 19 : if ((app.regmap < 0x0500) || (app.regmap >= 0x0600)) {
428 : #endif
429 0 : LOG_ERR("Wrong version of felix-star %s for this registermap %x", FELIX_TAG, app.regmap);
430 0 : return 1;
431 : }
432 :
433 39 : char cmem_name[255];
434 39 : snprintf(cmem_name, 255, "%s-%d-%u", basename(argv[0]), app.cfg.device, app.flx.dmaid);
435 39 : LOG_INFO("CMEM name: %s", cmem_name);
436 39 : app.dma_buffer.vmem = app.cfg.vmem;
437 39 : app.dma_buffer.free_previous_cmem = app.cfg.free_previous_cmem;
438 39 : flx_init_cmem_buffer(&app.dma_buffer, app.cfg.cmem_buffersize, cmem_name);
439 :
440 : // Selection of DMA transfer type
441 39 : if (app.cfg.cdma) {
442 2 : LOG_INFO("Circular ToFlx DMA selected");
443 2 : flx_dma_from_host(&app.flx, &app.dma_buffer, app.dma_buffer.paddr, app.dma_buffer.size, FLX_DMA_WRAPAROUND);
444 2 : app.dma_softw_ptr = flx_dma_get_read_ptr(&app.flx, &app.dma_buffer);
445 2 : LOG_DBG("dma_softw_ptr = 0x%08x", app.dma_softw_ptr);
446 2 : app.transfer_message = dma_circular_transfer;
447 : }
448 : else {
449 37 : LOG_INFO("Single-shot ToFlx DMA selected");
450 37 : app.transfer_message = dma_single_shot_transfer;
451 : }
452 :
453 : // Config value check
454 39 : if (app.cfg.netio_watermark > app.cfg.netio_pagesize){
455 0 : LOG_WARN("Netio watermark higher than pagesize, setting watermark to pagesize-1");
456 0 : app.cfg.netio_watermark = app.cfg.netio_pagesize - 1;
457 : }
458 39 : if (app.cfg.ttc_netio_watermark > app.cfg.ttc_netio_pagesize){
459 0 : LOG_WARN("Netio TTC2H watermark higher than pagesize, setting watermark to pagesize-1");
460 0 : app.cfg.ttc_netio_watermark = app.cfg.ttc_netio_pagesize - 1;
461 : }
462 :
463 39 : netio_init(&app.netio.ctx);
464 39 : app.netio.ctx.evloop.cb_init = on_init;
465 39 : netio_timer_init(&app.netio.ctx.evloop, &app.stats_writer.timer);
466 :
467 39 : LOG_INFO("Receiving data on %s:%d", app.cfg.ip, app.cfg.port);
468 39 : LOG_INFO("Connector Offset (co): 0x%07x (vid: 0x%x, did: 0x%x, cid: 0x%x)",
469 : app.cfg.co, app.cfg.vid,
470 : app.cfg.did, app.cfg.cid);
471 :
472 : // Read elink config info
473 39 : uint to_flx = 1;
474 39 : app.num_elinks = flx_read_elink_config(&app.flx, &app.elinks, to_flx);
475 39 : LOG_INFO("Num elinks: %u", app.num_elinks);
476 :
477 : // Notify BUS
478 39 : char bus_filename[255];
479 39 : snprintf(bus_filename, 255, "dma-%u", app.flx.dmaid);
480 39 : LOG_INFO("BUS Filename: %s", bus_filename);
481 39 : char* bus_path = felix_bus_path(app.cfg.bus_dir, app.cfg.bus_groupname, app.cfg.vid, app.cfg.did, app.cfg.cid, bus_filename);
482 39 : if (bus_path == NULL) {
483 0 : LOG_ERR("Could not create bus_path");
484 0 : return 1;
485 : }
486 39 : felix_bus bus = felix_bus_open(bus_path);
487 39 : if (!bus) {
488 0 : LOG_ERR("Could not open bus path %s", bus_path);
489 0 : return 1;
490 : }
491 :
492 39 : uint8_t pubsub = !to_flx;
493 39 : bus_write(bus, app.elinks, app.num_elinks, pubsub, app.cfg.unbuffered, &app.cfg);
494 39 : felix_bus_close(bus); // internally intentionally kept open until program terminates
495 :
496 39 : netio_run(&app.netio.ctx.evloop);
497 :
498 : //TODO free app.netio.buffers.data and app.netio.buffers
499 :
500 39 : flx_close_card(&app.flx);
501 39 : LOG_DBG("DMA stopped");
502 39 : log_flush();
503 :
504 39 : flx_close_cmem_buffer(&app.dma_buffer);
505 39 : LOG_DBG("CMEM buffer closed");
506 39 : log_flush();
507 :
508 39 : if(app.cfg.statistics_fifo) {
509 2 : statistics_free(&app.stats_writer);
510 : }
511 :
512 : return 0;
513 : }
|
