source: rtems/c/src/lib/libbsp/powerpc/qoriq/shmsupp/intercom.c @ 44ba969

Last change on this file since 44ba969 was 43cc2b4, checked in by Sebastian Huber <sebastian.huber@…>, on Aug 10, 2017 at 9:39:43 AM

bsp/qoriq: Add basic 64-bit support

Update #3082.

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