source: rtems/c/src/lib/libbsp/powerpc/qoriq/shmsupp/intercom.c @ 0df59b7c

5
Last change on this file since 0df59b7c was 0df59b7c, checked in by Sebastian Huber <sebastian.huber@…>, on 01/16/18 at 10:40:17

bsp/qoriq: Optional multiprocessing support

Update #3085.

  • Property mode set to 100644
File size: 11.5 KB
Line 
1/**
2 * @file
3 *
4 * @ingroup QorIQInterCom
5 *
6 * @brief Inter-Processor Communication implementation.
7 */
8
9/*
10 * Copyright (c) 2011 embedded brains GmbH.  All rights reserved.
11 *
12 *  embedded brains GmbH
13 *  Obere Lagerstr. 30
14 *  82178 Puchheim
15 *  Germany
16 *  <rtems@embedded-brains.de>
17 *
18 * The license and distribution terms for this file may be
19 * found in the file LICENSE in this distribution or at
20 * http://www.rtems.org/license/LICENSE.
21 */
22
23#include <assert.h>
24#include <string.h>
25
26#include <rtems.h>
27
28#include <libcpu/powerpc-utility.h>
29
30#include <bspopts.h>
31#include <bsp/irq.h>
32#include <bsp/qoriq.h>
33#include <bsp/intercom.h>
34
35#ifdef RTEMS_MULTIPROCESSING
36
37#define INTERCOM_EVENT_IPI RTEMS_EVENT_13
38#define INTERCOM_EVENT_WAKE_UP RTEMS_EVENT_14
39
40#define PACKET_SIZE_COUNT 4
41
42#define ONE_CORE(core) (1U << (core))
43#define ALL_CORES ((1U << INTERCOM_CORE_COUNT) - 1U)
44#define OTHER_CORES(core) (ALL_CORES & ~ONE_CORE(core))
45
46#define IPI_INDEX 0
47
48typedef struct consumer {
49        struct consumer *next;
50        rtems_id task;
51} consumer;
52
53typedef struct {
54        consumer *head;
55        uint32_t cache_line_alignment [7];
56} consumer_list;
57
58typedef struct {
59        uint32_t lock;
60        intercom_packet *head;
61        size_t size;
62        uint32_t cores_to_notify;
63        uint32_t cache_line_alignment [4];
64        consumer_list waiting_consumers [INTERCOM_CORE_COUNT];
65} free_list;
66
67typedef struct {
68        uint32_t lock;
69        intercom_packet *head;
70        intercom_packet *tail;
71        uint32_t cache_line_alignment [5];
72} core_fifo;
73
74typedef struct {
75        free_list free_lists [PACKET_SIZE_COUNT];
76        core_fifo core_fifos [INTERCOM_CORE_COUNT];
77        intercom_service services [INTERCOM_CORE_COUNT][INTERCOM_SERVICE_COUNT];
78        void *service_args [INTERCOM_CORE_COUNT][INTERCOM_SERVICE_COUNT];
79        uint32_t ready_lock;
80        uint32_t ready;
81        uint32_t cache_line_alignment [6];
82} control;
83
84static control *const intercom = (control *) QORIQ_INTERCOM_AREA_BEGIN;
85
86static const size_t packet_sizes [PACKET_SIZE_COUNT] = {
87        64,
88        512,
89        2048,
90        4096
91};
92
93static void send_event(rtems_id task, rtems_event_set event)
94{
95        rtems_status_code sc = RTEMS_SUCCESSFUL;
96
97        sc = rtems_event_send(task, event);
98        assert(sc == RTEMS_SUCCESSFUL);
99}
100
101static void wait_for_event(rtems_event_set in)
102{
103        rtems_status_code sc = RTEMS_SUCCESSFUL;
104        rtems_event_set out;
105
106        sc = rtems_event_receive(
107                in,
108                RTEMS_EVENT_ALL | RTEMS_WAIT,
109                RTEMS_NO_TIMEOUT,
110                &out
111        );
112        assert(sc == RTEMS_SUCCESSFUL);
113}
114
115static void intercom_handler(void *arg)
116{
117        rtems_id task = (rtems_id) (uintptr_t) arg;
118        send_event(task, INTERCOM_EVENT_IPI);
119}
120
121static void notify_core_by_index(int core)
122{
123        uint32_t self = ppc_processor_id();
124        qoriq.pic.per_cpu [self].ipidr [IPI_INDEX].reg = ONE_CORE(core);
125}
126
127static void notify_cores(uint32_t cores)
128{
129        uint32_t self = ppc_processor_id();
130        qoriq.pic.per_cpu [self].ipidr [IPI_INDEX].reg = cores;
131}
132
133void qoriq_intercom_free_packet(intercom_packet *packet)
134{
135        free_list *list = &intercom->free_lists [packet->size_index];
136
137        uint32_t msr = qoriq_spin_lock(&list->lock);
138        intercom_packet *first = list->head;
139        list->head = packet;
140        packet->glue.next = first;
141        uint32_t cores = list->cores_to_notify;
142        if (cores != 0) {
143                list->cores_to_notify = 0;
144                notify_cores(cores);
145        }
146        qoriq_spin_unlock(&list->lock, msr);
147}
148
149static void default_service(intercom_packet *packet, void *arg)
150{
151        qoriq_intercom_free_packet(packet);
152}
153
154static void process_free_lists(free_list *free_lists, uint32_t self)
155{
156        int i = 0;
157
158        for (i = 0; i < PACKET_SIZE_COUNT; ++i) {
159                free_list *list = &free_lists [i];
160
161                uint32_t msr = qoriq_spin_lock(&list->lock);
162                consumer *waiting_consumer = list->waiting_consumers [self].head;
163                list->waiting_consumers [self].head = NULL;
164                qoriq_spin_unlock(&list->lock, msr);
165
166                while (waiting_consumer != NULL) {
167                        send_event(waiting_consumer->task, INTERCOM_EVENT_WAKE_UP);
168                        waiting_consumer = waiting_consumer->next;
169                }
170        }
171}
172
173static void process_core_fifo(core_fifo *fifo, intercom_service *services, void **service_args)
174{
175        uint32_t msr = qoriq_spin_lock(&fifo->lock);
176        intercom_packet *packet = fifo->head;
177        fifo->head = NULL;
178        qoriq_spin_unlock(&fifo->lock, msr);
179
180        while (packet != NULL) {
181                intercom_packet *current = packet;
182                intercom_type type_index = current->type_index;
183                packet = current->glue.next;
184                (*services [type_index])(current, service_args [type_index]);
185        }
186}
187
188static void intercom_task(rtems_task_argument arg)
189{
190        uint32_t self = ppc_processor_id();
191        free_list *free_lists = &intercom->free_lists [0];
192        intercom_service *services = &intercom->services [self][0];
193        void **service_args = &intercom->service_args [self][0];
194        core_fifo *fifo = &intercom->core_fifos [self];
195
196        while (true) {
197                process_free_lists(free_lists, self);
198                process_core_fifo(fifo, services, service_args);
199                wait_for_event(INTERCOM_EVENT_IPI);
200        }
201}
202
203static intercom_packet *free_list_and_packet_init(
204        free_list *list,
205        size_t count,
206        intercom_packet *current,
207        intercom_size size_index,
208        size_t size
209)
210{
211        intercom_packet *last = current;
212        size_t inc = 1 + size / sizeof(*current);
213        size_t i = 0;
214
215        assert(count > 0);
216        assert(size % sizeof(*current) == 0);
217
218        list->size = size;
219        list->head = current;
220        for (i = 0; i < count; ++i) {
221                intercom_packet *next = current + inc;
222                current->glue.next = next;
223                current->size_index = size_index;
224                last = current;
225                current = next;
226        }
227        last->glue.next = NULL;
228
229        return current;
230}
231
232static void basic_init(void)
233{
234        char *begin = (char *) QORIQ_INTERCOM_AREA_BEGIN;
235        size_t size = QORIQ_INTERCOM_AREA_SIZE;
236        int i = 0;
237
238        memset(begin, 0, size);
239
240        assert(size % packet_sizes [PACKET_SIZE_COUNT - 1] == 0);
241
242        /* Calculate data area sizes */
243        size_t data_sizes [PACKET_SIZE_COUNT];
244        data_sizes [PACKET_SIZE_COUNT - 1] = size / 2;
245        for (i = PACKET_SIZE_COUNT - 2; i > 0; --i) {
246                data_sizes [i] = data_sizes [i + 1] / 2;
247        }
248        data_sizes [i] = data_sizes [i + 1];
249
250        /* Calculate packet counts */
251        size_t packet_counts [PACKET_SIZE_COUNT];
252        size_t count = 0;
253        for (i = 1; i < PACKET_SIZE_COUNT; ++i) {
254                packet_counts [i] = data_sizes [i] / packet_sizes [i];
255                count += packet_counts [i];
256        }
257        packet_counts [0] = (data_sizes [0] - sizeof(control) - count * sizeof(intercom_packet))
258                / (sizeof(intercom_packet) + packet_sizes [0]);
259
260        /* Initialize free lists and packets */
261        intercom_packet *packet = (intercom_packet *) (begin + sizeof(control));
262        for (i = 0; i < PACKET_SIZE_COUNT; ++i) {
263                packet = free_list_and_packet_init(
264                        &intercom->free_lists [i],
265                        packet_counts [i],
266                        packet,
267                        i,
268                        packet_sizes [i]
269                );
270        }
271
272        rtems_cache_flush_multiple_data_lines(begin, size);
273        ppc_synchronize_data();
274}
275
276static void services_init(uint32_t self)
277{
278        int i = 0;
279
280        for (i = 0; i < INTERCOM_SERVICE_COUNT; ++i) {
281                if (intercom->services [self][i] == NULL) {
282                        intercom->services [self][i] = default_service;
283                }
284        }
285}
286
287void qoriq_intercom_init(void)
288{
289        rtems_status_code sc = RTEMS_SUCCESSFUL;
290        rtems_id task = RTEMS_ID_NONE;
291        uint32_t self = ppc_processor_id();
292
293        sc = rtems_task_create(
294                rtems_build_name('I', 'C', 'O', 'M'),
295                10,
296                0,
297                RTEMS_DEFAULT_MODES,
298                RTEMS_DEFAULT_ATTRIBUTES,
299                &task
300        );
301        assert(sc == RTEMS_SUCCESSFUL);
302
303        sc = qoriq_pic_set_priority(
304                QORIQ_IRQ_IPI_0,
305                QORIQ_PIC_PRIORITY_LOWEST,
306                NULL
307        );
308        assert(sc == RTEMS_SUCCESSFUL);
309
310        sc = rtems_interrupt_handler_install(
311                QORIQ_IRQ_IPI_0,
312                "INTERCOM",
313                RTEMS_INTERRUPT_UNIQUE,
314                intercom_handler,
315                (void *) (uintptr_t) task
316        );
317        assert(sc == RTEMS_SUCCESSFUL);
318
319        if (self == 0) {
320                basic_init();
321        }
322
323        services_init(self);
324
325        sc = rtems_task_start(task, intercom_task, 0);
326        assert(sc == RTEMS_SUCCESSFUL);
327}
328
329void qoriq_intercom_start(void)
330{
331        uint32_t self = ppc_processor_id();
332        uint32_t ready = 0;
333
334        while (ready != ALL_CORES) {
335                uint32_t msr = qoriq_spin_lock(&intercom->ready_lock);
336                ready = intercom->ready;
337                intercom->ready = ready | ONE_CORE(self);
338                qoriq_spin_unlock(&intercom->ready_lock, msr);
339        }
340}
341
342static intercom_packet *allocate(intercom_type type, free_list *list)
343{
344        uint32_t self = ppc_processor_id();
345        intercom_packet *packet = NULL;
346        consumer poor = {
347                .task = rtems_task_self()
348        };
349
350        while (packet == NULL) {
351                uint32_t msr = qoriq_spin_lock(&list->lock);
352                packet = list->head;
353                if (packet != NULL) {
354                        list->head = packet->glue.next;
355                } else {
356                        consumer *first = list->waiting_consumers [self].head;
357                        list->waiting_consumers [self].head = &poor;
358                        poor.next = first;
359                        if (first == NULL) {
360                                list->cores_to_notify |= ONE_CORE(self);
361                        }
362                }
363                qoriq_spin_unlock(&list->lock, msr);
364
365                if (packet == NULL) {
366                        wait_for_event(INTERCOM_EVENT_WAKE_UP);
367                }
368        }
369
370        packet->glue.next = NULL;
371        packet->type_index = type;
372        packet->flags = 0;
373        packet->size = list->size;
374
375        return packet;
376}
377
378intercom_packet *qoriq_intercom_allocate_packet(intercom_type type, intercom_size size)
379{
380        assert((unsigned) type < INTERCOM_SERVICE_COUNT);
381        assert((unsigned) size < PACKET_SIZE_COUNT);
382
383        return allocate(type, &intercom->free_lists [size]);
384}
385
386void qoriq_intercom_send_packets(int destination_core, intercom_packet *first, intercom_packet *last)
387{
388        assert(destination_core >= 0);
389        assert(destination_core < INTERCOM_CORE_COUNT);
390
391        core_fifo *fifo = &intercom->core_fifos [destination_core];
392
393        uint32_t msr = qoriq_spin_lock(&fifo->lock);
394        last->glue.next = NULL;
395        if (fifo->head != NULL) {
396                fifo->tail->glue.next = first;
397        } else {
398                fifo->head = first;
399                notify_core_by_index(destination_core);
400        }
401        fifo->tail = last;
402        qoriq_spin_unlock(&fifo->lock, msr);
403}
404
405void qoriq_intercom_broadcast_packets(intercom_packet *first, intercom_packet *last)
406{
407        int i = 0;
408
409        for (i = 1; i < INTERCOM_CORE_COUNT; ++i) {
410                intercom_packet *clone_first = NULL;
411                intercom_packet *clone_last = NULL;
412
413                intercom_packet *current = first;
414                while (current != NULL) {
415                        intercom_packet *clone = qoriq_intercom_clone_packet(current);
416                        if (clone_first == NULL) {
417                                clone_first = clone;
418                        }
419                        if (clone_last != NULL) {
420                                clone_last->glue.next = clone;
421                        }
422                        clone_last = clone;
423                        current = current->glue.next;
424                }
425
426                qoriq_intercom_send_packets(i, clone_first, clone_last);
427        }
428
429        qoriq_intercom_send_packets(0, first, last);
430}
431
432void qoriq_intercom_send(int destination_core, intercom_type type, intercom_size size, const void *buf, size_t n)
433{
434        assert((unsigned) size < PACKET_SIZE_COUNT);
435
436        size_t remaining = n;
437        size_t packet_size = packet_sizes [size];
438        const char *src = buf;
439        intercom_packet *first = NULL;
440        intercom_packet *last = NULL;
441
442        do {
443                intercom_packet *packet = qoriq_intercom_allocate_packet(
444                        type,
445                        size
446                );
447                if (first == NULL) {
448                        first = packet;
449                }
450                if (last != NULL) {
451                        last->glue.next = packet;
452                }
453                last = packet;
454                size_t current_size = remaining < packet_size ? remaining : packet_size;
455                remaining -= current_size;
456                packet->size = current_size;
457                const char *current = src;
458                src += current_size;
459                memcpy(packet->data, current, current_size);
460        } while (remaining > 0);
461
462        qoriq_intercom_send_packets(destination_core, first, last);
463}
464
465void qoriq_intercom_service_install(intercom_type type, intercom_service service, void *arg)
466{
467        assert((unsigned) type < INTERCOM_SERVICE_COUNT);
468
469        uint32_t self = ppc_processor_id();
470        intercom->service_args [self][type] = arg;
471        ppc_enforce_in_order_execution_of_io();
472        intercom->services [self][type] = service;
473}
474
475void qoriq_intercom_service_remove(intercom_type type)
476{
477        assert((unsigned) type < INTERCOM_SERVICE_COUNT);
478
479        uint32_t self = ppc_processor_id();
480        intercom->services [self][type] = default_service;
481        ppc_enforce_in_order_execution_of_io();
482        intercom->service_args [self][type] = NULL;
483}
484
485intercom_packet *qoriq_intercom_clone_packet(const intercom_packet *packet)
486{
487        intercom_packet *clone = qoriq_intercom_allocate_packet(
488                packet->type_index,
489                packet->size_index
490        );
491
492        clone->size = packet->size;
493        memcpy(clone->data, packet->data, clone->size);
494
495        return clone;
496}
497
498#endif /* RTEMS_MULTIPROCESSING */
Note: See TracBrowser for help on using the repository browser.