source: trunk/tools/alcons/threads.c @ 547

Last change on this file since 547 was 231, checked in by max@…, 7 years ago

Add a serial port multiplexer, usable to communicate with the
ALMOS-MKH kernel over a serial port.

File size: 11.5 KB
Line 
1/*
2 *******************************************************************************
3 * Authors:
4 *     Maxime Villard, 2017
5 *     Jankovic Marco, 01/07/2014
6 *******************************************************************************
7 */
8
9#include "common.h"
10
11typedef enum fsm_state_t {
12        FSM_INIT_COM,
13        FSM_IDLE,
14        FSM_READ_TTY,
15        FSM_WRITE_TTY,
16        FSM_READ_DEVICE,
17        FSM_WRITE_DEVICE,
18        FSM_READ_FIFO_RX,
19        FSM_READ_FIFO_TX,
20        FSM_WRITE_FIFO_RX,
21        FSM_WRITE_FIFO_TX,
22} fsm_state;
23
24typedef enum flux_control_state_t {
25        DONE,
26        SEND_XON,
27        SEND_XOFF
28} flux_control_state;
29
30typedef enum tx_state_t {
31        ENABLE,
32        DISABLE,
33} tx_state;
34
35/*
36 * Variables used for thread communication.
37 */
38static flux_control_state flux_control = DONE;  // used for xon/xoff
39static tx_state TX_STATE = ENABLE;                              // used to ENABLE/DISABLE thread write
40
41/*
42 * This thread reads the fifo_rx for data, then writes those data to the correct
43 * tty with the id contained in the packet. Then it checks if a tty is readable.
44 * If there is one, write N packet from one tty to the fifo_tx then return to
45 * idle state.
46 */
47void *thread_mux_demux(void *a)
48{
49        UNUSED(a);
50        printf("[+] thread_mux launched\n");
51
52        fd_set readfs;
53        char packet_buffer[2 * READ_LEN];
54        char tty_raw_buffer[READ_LEN];
55        char *pkt;
56
57        int res;
58        int i,j;
59        int tty_id;
60        int tty_size_read = 0;
61        int fifo_size_read = 0;
62        fsm_state mux_state;
63
64        sigset_t ens;
65        sigfillset(&ens);
66        sigdelset(&ens, SIGTERM);
67        pthread_sigmask(SIG_SETMASK, &ens, NULL);
68
69        struct timeval timeout;
70
71        mux_state = FSM_IDLE;
72
73        while (1)
74        {
75                switch (mux_state) {
76                        /*
77                         * check if the fifo_rx is readable -> FSM_READ_FIFO_RX
78                         * if not check if there is readable data in target_fd -> FSM_READ_TTY
79                         */
80                        case FSM_IDLE:
81                                FD_ZERO(&readfs);
82                                FD_SET(fifo_rx_fd, &readfs);
83
84                                /*
85                                 * check if fifo_rx is readable
86                                 */
87                                timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
88                                timeout.tv_usec = TIMEOUT_DELAY_MUX_US;
89                                res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
90                                if (res < 0) {
91                                        err(1, "select");
92                                } else if (res == 0) {
93                                        /* nothing to read, try to read a tty */
94                                        mux_state = FSM_READ_TTY;
95                                        break;
96                                }
97
98                                /* readable */
99                                mux_state = FSM_READ_FIFO_RX;
100                                break;
101
102                        /*
103                         * Read fifo_rx.
104                         */
105                        case FSM_READ_FIFO_RX:
106                                fifo_size_read = read(fifo_rx_fd, packet_buffer, READ_LEN);
107
108                                if (!fifo_size_read) {
109                                        perror("read");
110                                        mux_state = FSM_IDLE;
111                                        break;
112                                }
113
114                                mux_state = FSM_WRITE_TTY;
115                                break;
116
117                        /*
118                         * Write the data to the proper tty, then -> FSM_READ_TTY.
119                         */
120                        case FSM_WRITE_TTY:
121                                j = 0;
122
123                                while (j < fifo_size_read) {
124                                        pkt = packet_buffer + j;
125                                        write(slave_fd[(uint8_t)pkt[PKT_TTY]], &pkt[PKT_DAT], 1);
126#if DEBUG_MUX
127                                        printf("[MUX::FSM_WRITE_TTY] tty_id=%x | data=%c\n",
128                                            pkt[PKT_TTY], pkt[PKT_DAT]);
129#endif
130                                        j += PACKET_SIZE;
131                                }
132                                mux_state = FSM_READ_TTY;
133                                break;
134
135                        /*
136                         * Check if there is a readable tty. Return to idle if there is no
137                         * tty ready, otherwise -> FSM_WRITE_FIFO_TX.
138                         */
139                        case FSM_READ_TTY:
140                                timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
141                                timeout.tv_usec = TIMEOUT_DELAY_MUX_US;
142
143                                FD_ZERO(&readfs);
144                                for (i = 0; i < NB_CHANNELS; i++) {
145                                        FD_SET(slave_fd[i], &readfs);
146                                }
147
148                                res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
149                                if (res < 0) {
150                                        err(1, "select");
151                                } else if (res == 0) {
152                                        /* no readable tty */
153                                        mux_state = FSM_IDLE;
154                                        break;
155                                }
156
157                                /* one tty is readable, find it */
158                                tty_id = 0;
159                                while (tty_id < NB_CHANNELS && !(res = FD_ISSET(slave_fd[tty_id], &readfs))) {
160                                        tty_id++;
161                                }
162
163                                /* read it */
164                                tty_size_read = read(slave_fd[tty_id], tty_raw_buffer, READ_LEN);
165
166                                if (tty_size_read == 0) {
167                                        mux_state = FSM_IDLE;
168                                        printf("thread MUX FSM_READ_TTY: error while reading tty_%d\n", tty_id);
169                                        perror("read");
170                                        break;
171                                }
172#if DEBUG_MUX
173                                printf("[MUX::FSM_READ_TTY] read from tty_id=%d | %d bytes\n",
174                                    tty_id, tty_size_read);
175#endif
176                                i = 0;
177                                j = 0;
178                                while (i < tty_size_read) {
179                                        pkt = packet_buffer + j;
180                                        pkt[PKT_TTY] = tty_id | 0x80;
181                                        pkt[PKT_DAT] = tty_raw_buffer[i];
182                                        i++;
183                                        j += PACKET_SIZE;
184                                }
185
186                                mux_state = FSM_WRITE_FIFO_TX;
187                                break;
188
189                        /*
190                         * Write packet_buffer into fifo_tx.
191                         */
192                        case FSM_WRITE_FIFO_TX:
193                                write(fifo_tx_fd, packet_buffer, tty_size_read * PACKET_SIZE);
194                                mux_state = FSM_IDLE;
195                                break;
196
197                        default:
198                                err(1, "unreachable");
199                                break;
200                }
201        }
202        return (void*)NULL;
203}
204
205/*
206 * This thread initiates communication with the device by sending XON
207 * -> read the RX_BUFFER
208 * -> check packet correctness
209 * -> check fifo_rx size
210 * -> software flux control
211 * -> then write to the fifo_rx
212 */
213void *thread_read_rx(void *a)
214{
215        UNUSED(a);
216        printf("[+] thread_read_rx launched\n");
217        int size_read = 0;
218        int bytes_avail = 0;
219        int id_ok = 0;
220        int res = 0;
221        int x_off = 0;
222        int device_raw_offset = 0;
223        fd_set readfs;
224        int fd;
225
226#if TEST_BENCH
227        fd = test_rx_fd;
228#else
229        fd = device_fd;
230#endif
231
232        unsigned char device_raw_data[READ_LEN];
233        unsigned char packet_buffer[2];
234
235        struct timeval timeout;
236
237#if TEST_BENCH
238        fsm_state read_state = FSM_IDLE;
239#else
240        fsm_state read_state = FSM_INIT_COM;
241#endif
242
243        while (1)
244        {
245                switch (read_state)
246                {
247                        /*
248                         * This state is used to initiate communication with the device
249                         * by sending periodically XON while the RX_BUFFER is empty.
250                         * Then we reach FSM_IDLE, and never to this state afterwards.
251                         */
252                        case FSM_INIT_COM:
253                                FD_ZERO(&readfs);
254                                FD_SET(fd, &readfs);
255
256                                timeout.tv_sec = TIMEOUT_DELAY_READ_S;
257                                timeout.tv_usec = TIMEOUT_DELAY_READ_US;
258
259                                res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
260                                if (res > 0) {
261                                        read_state = FSM_IDLE;
262                                        flux_control = DONE;
263                                        break;
264                                } else if (res < 0) {
265                                        err(1, "select");
266                                } else {
267                                        /* nothing to read */
268                                }
269
270                                if (flux_control == DONE)
271                                        flux_control = SEND_XON;
272                                break;
273
274                        /*
275                         * This state first checks the number of bytes available in fifo_rx.
276                         * If FIFO_LIMIT_SIZE is reached a XOFF is sent. After checking the
277                         * fifo, check if the device is ready for i/o -> FSM_READ_DEVICE.
278                         */
279                        case FSM_IDLE:
280                                FD_ZERO(&readfs);
281                                FD_SET(fd, &readfs);
282
283                                if (ioctl(fifo_rx_fd, FIONREAD, &bytes_avail) < 0) {
284                                        err(1, "ioctl");
285                                }
286
287                                if (bytes_avail > FIFO_LIMIT_SIZE && !x_off) {
288                                        if (flux_control == DONE) {
289                                                flux_control = SEND_XOFF;
290                                                x_off = 1;
291                                        }
292                                } else if ((bytes_avail < FIFO_LIMIT_SIZE) && x_off) {
293                                        if (flux_control == DONE) {
294                                                flux_control = SEND_XON;
295                                                x_off = 0;
296                                        }
297                                }
298
299                                /*
300                                 * check if the device is readable
301                                 */
302                                timeout.tv_sec = TIMEOUT_DELAY_READ_S;
303                                timeout.tv_usec = TIMEOUT_DELAY_READ_US;
304                                res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
305                                if (res > 0) {
306                                        read_state = FSM_READ_DEVICE;
307                                        break;
308                                } else if (res < 0) {
309                                        err(1, "select");
310                                } else {
311                                        /* nothing to read */
312                                }
313
314                                break;
315
316                                /*
317                                 * Read the device, then -> FSM_WRITE_FIFO_RX.
318                                 */
319                        case FSM_READ_DEVICE:
320                                size_read = read(fd, device_raw_data, READ_LEN);
321                                if (size_read == 0) {
322                                        read_state = FSM_IDLE;
323                                        break;
324                                }
325
326                                read_state = FSM_WRITE_FIFO_RX;
327                                break;
328
329                                /*
330                                 * Control packet correctness
331                                 * if a data is XON -> ENABLE thread_write
332                                 * if a data is XOFF -> DISABLE thread_write
333                                 * non correct packet are dropped
334                                 * then back to idle
335                                 */
336                        case FSM_WRITE_FIFO_RX:
337                                device_raw_offset = 0;
338                                while (device_raw_offset < size_read)
339                                {
340#if DEBUG_READ
341                                        printf("-> got packet from device: data=%x\n", device_raw_data[device_raw_offset]);
342#endif
343                                        if (device_raw_data[device_raw_offset] == XOFF) {
344                                                printf("XOFF received\n");
345                                                TX_STATE = DISABLE;
346                                                device_raw_offset++;
347                                                continue;
348                                        } else if (device_raw_data[device_raw_offset] == XON) {
349                                                printf("XON received\n");
350                                                TX_STATE = ENABLE;
351                                                device_raw_offset++;
352                                                continue;
353                                        } else if (device_raw_data[device_raw_offset] & 0x80) {
354                                                id_ok = 1;
355                                                packet_buffer[PKT_TTY] = device_raw_data[device_raw_offset] & 0x7F; /* XXX: should be in the demux thread */
356
357                                                device_raw_offset++;
358#if DEBUG_READ
359                                                printf("--> detected ID in packet: id=%x\n", packet_buffer[PKT_TTY]);
360#endif
361                                                continue;
362                                        }
363                                        /* check if data is not ID*/
364                                        else if (id_ok && !(device_raw_data[device_raw_offset] & 0x80))
365                                        {
366                                                if (device_raw_data[device_raw_offset] == XOFF) {
367#if DEBUG_READ
368                                                        printf("--> detected DATA in packet: XOFF\n");
369#endif
370                                                        device_raw_offset++;
371                                                        TX_STATE = DISABLE;
372                                                        continue;
373                                                } else if (device_raw_data[device_raw_offset] == XON) {
374#if DEBUG_READ
375                                                        printf("--> detected DATA in packet: XON\n");
376#endif
377                                                        device_raw_offset++;
378                                                        TX_STATE = ENABLE;
379                                                        continue;
380                                                }
381#if DEBUG_READ
382                                                printf("--> detected DATA in packet: '%c'\n", device_raw_data[device_raw_offset]);
383#endif
384                                                packet_buffer[PKT_DAT] = device_raw_data[device_raw_offset];
385                                                id_ok = 0;
386
387                                                write(fifo_rx_fd, packet_buffer, PACKET_SIZE);
388                                                device_raw_offset++;
389                                                continue;
390                                        }
391                                        printf("-> dropping data=%x\n", device_raw_data[device_raw_offset]);
392                                        device_raw_offset++;
393                                }
394
395                                read_state = FSM_IDLE;
396                                break;
397
398                        default:
399                                err(1, "unreachable");
400                                break;
401                }
402        }
403        return NULL;
404}
405
406/*
407 * This thread writes packets from the fifo_tx to the TX_BUFFER. This thread is
408 * controlled with TX_STATE and flux_control.
409 */
410void *thread_write_tx(void *a)
411{
412        UNUSED(a);
413#if DEBUG_WRITE
414        int i,j;
415#endif
416
417        int res = 0;
418        int size_read = 0;
419        int fd;
420
421        unsigned char xon = XON;
422        unsigned char xoff = XOFF;
423        fd_set readfs;
424
425#if TEST_BENCH
426        fd = test_tx_fd;
427#else
428        fd = device_fd;
429#endif
430
431        unsigned char device_buf[READ_LEN];
432
433        struct timeval timeout;
434
435        fsm_state write_state = FSM_IDLE;
436
437        printf("[+] thread_write_tx launched\n");
438
439        while (1)
440        {
441                switch (write_state)
442                {
443                        /*
444                         * Depending on TX_STATE and flux_control, send a xon or xoff or
445                         * yield if DISABLE. Otherwise, check if the fifo_tx is readable
446                         * -> FSM_READ_FIFO_TX.
447                         */
448                        case FSM_IDLE:
449                                /* software flux control section */
450                                if (TX_STATE == DISABLE && flux_control != DONE) {
451                                        sched_yield();
452                                        break;
453                                } else if (flux_control == SEND_XOFF) {
454                                        write(fd, &xoff, 1);
455                                        flux_control = DONE;
456                                        printf("XOFF sent\n");
457                                        break;
458                                } else if (flux_control == SEND_XON) {
459                                        write(fd, &xon, 1);
460                                        flux_control = DONE;
461                                        printf("XON sent\n");
462                                        break;
463                                }
464                                /**********************************/
465
466                                /*
467                                 * check if the fifo is readable
468                                 */
469                                FD_ZERO(&readfs);
470                                FD_SET(fifo_tx_fd, &readfs);
471                                timeout.tv_sec = TIMEOUT_DELAY_WRITE_S;
472                                timeout.tv_usec = TIMEOUT_DELAY_WRITE_US;
473                                res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
474                                if (res > 0) {
475                                        if (TX_STATE != DISABLE)
476                                                write_state = FSM_READ_FIFO_TX;
477                                        break;
478                                } else if (res < 0) {
479                                        err(1, "select");
480                                } else {
481                                        /* nothing to read */
482                                }
483
484                                break;
485
486                        /*
487                         * Read from fifo_tx, then -> FSM_WRITE_DEVICE.
488                         */
489                        case FSM_READ_FIFO_TX:
490                                size_read = read(fifo_tx_fd, device_buf, READ_LEN);
491
492#if DEBUG_WRITE
493                                i = 0;
494                                j = 0;
495                                while (i < size_read) {
496                                        printf("thread WRITE id=%x data='%c'\n", device_buf[j + PKT_TTY],device_buf[j + PKT_DAT]);
497                                        i += PACKET_SIZE;
498                                        j += PACKET_SIZE;
499                                }
500#endif
501
502                                if (size_read == 0) {
503                                        write_state = FSM_IDLE;
504                                        break;
505                                }
506
507                                write_state = FSM_WRITE_DEVICE;
508                                break;
509
510                                /*
511                                 * Write to the device, then return to idle.
512                                 */
513                        case FSM_WRITE_DEVICE:
514                                size_read = write(fd, device_buf, size_read);
515                                write_state = FSM_IDLE;
516                                break;
517
518                        default:
519                                err(1, "unreachable");
520                                break;
521                }
522        }
523        return NULL;
524}
Note: See TracBrowser for help on using the repository browser.