--- .../libbsp/powerpc/motorola_powerpc/Makefile.am | 2 + .../libbsp/powerpc/motorola_powerpc/include/bsp.h | 8 + .../libbsp/powerpc/motorola_powerpc/shmsupp/mpci.c | 375 +++++++++++++++++++++ c/src/lib/libbsp/powerpc/shared/startup/bspstart.c | 10 +- 4 files changed, 393 insertions(+), 2 deletions(-) create mode 100644 c/src/lib/libbsp/powerpc/motorola_powerpc/shmsupp/mpci.c
diff --git a/c/src/lib/libbsp/powerpc/motorola_powerpc/Makefile.am b/c/src/lib/libbsp/powerpc/motorola_powerpc/Makefile.am index e09820608..3576b75 100644 --- a/c/src/lib/libbsp/powerpc/motorola_powerpc/Makefile.am +++ b/c/src/lib/libbsp/powerpc/motorola_powerpc/Makefile.am @@ -136,6 +136,8 @@ noinst_PROGRAMS += ne2000.rel ne2000_rel_SOURCES = ../../i386/pc386/ne2000/ne2000.c ne2000_rel_CPPFLAGS = $(AM_CPPFLAGS) $(ne2000_CPPFLAGS) ne2000_rel_LDFLAGS = $(RTEMS_RELLDFLAGS) + +libbsp_a_SOURCES += shmsupp/mpci.c endif endif diff --git a/c/src/lib/libbsp/powerpc/motorola_powerpc/include/bsp.h b/c/src/lib/libbsp/powerpc/motorola_powerpc/include/bsp.h index 0439e87..b955d72 100644 --- a/c/src/lib/libbsp/powerpc/motorola_powerpc/include/bsp.h +++ b/c/src/lib/libbsp/powerpc/motorola_powerpc/include/bsp.h @@ -242,6 +242,14 @@ void zero_bss(void); */ void VIA_isa_bridge_interrupts_setup(void); +#ifdef RTEMS_MULTIPROCESSING + +extern rtems_mpci_table net_mpci_table; + +#define CONFIGURE_MP_MPCI_TABLE_POINTER &net_mpci_table + +#endif /* RTEMS_MULTIPROCESSING */ + #endif #ifdef __cplusplus diff --git a/c/src/lib/libbsp/powerpc/motorola_powerpc/shmsupp/mpci.c b/c/src/lib/libbsp/powerpc/motorola_powerpc/shmsupp/mpci.c new file mode 100644 index 0000000..8ec5fd5 --- /dev/null +++ b/c/src/lib/libbsp/powerpc/motorola_powerpc/shmsupp/mpci.c @@ -0,0 +1,375 @@ +/* + * Copyright (c) 2016 embedded brains GmbH. All rights reserved. + * + * embedded brains GmbH + * Dornierstr. 4 + * 82178 Puchheim + * Germany + * <rt...@embedded-brains.de> + * + * The license and distribution terms for this file may be + * found in the file LICENSE in this distribution or at + * http://www.rtems.org/license/LICENSE. + */ + +#include <bsp.h> + +#include <sys/select.h> +#include <sys/socket.h> + +#include <assert.h> +#include <inttypes.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> + +#include <arpa/inet.h> +#include <netinet/in.h> + +#include <rtems/chain.h> +#include <rtems/rtems_bsdnet.h> +#include <rtems/score/objectimpl.h> + +#define MAX_PACKET_SIZE 512 + +#define MAX_NODES 2 + +#define PACKET_COUNT (32 * MAX_NODES) + +#define IP_ADDR_SIZE 16 + +#define MPCI_PORT 1234 + +typedef struct { + rtems_chain_node node; + uint32_t dest; + char data[MAX_PACKET_SIZE]; +} net_mpci_packet; + +typedef struct { + rtems_chain_control outbound_packets; + rtems_id outbound_server; + RTEMS_INTERRUPT_LOCK_MEMBER(outbound_lock) + rtems_chain_control free_packets; + RTEMS_INTERRUPT_LOCK_MEMBER(free_lock) + int outbound_fds[MAX_NODES]; + int inbound_fds[MAX_NODES - 1]; + int inbound_nfds; + net_mpci_packet packets[PACKET_COUNT]; + char ip_addr[IP_ADDR_SIZE]; +} net_mpci_context; + +static net_mpci_context net_mpci_instance; + +static net_mpci_packet *net_mpci_get_outbound_packet(net_mpci_context *ctx) +{ + rtems_interrupt_lock_context lock_context; + rtems_chain_node *node; + + rtems_interrupt_lock_acquire(&ctx->outbound_lock, &lock_context); + node = rtems_chain_get_unprotected(&ctx->outbound_packets); + rtems_interrupt_lock_release(&ctx->outbound_lock, &lock_context); + + return (net_mpci_packet *) node; +} + +static void net_mpci_free_packet( + net_mpci_context *ctx, + net_mpci_packet *packet +) +{ + rtems_interrupt_lock_context lock_context; + + rtems_interrupt_lock_acquire(&ctx->free_lock, &lock_context); + rtems_chain_prepend_unprotected(&ctx->free_packets, &packet->node); + rtems_interrupt_lock_release(&ctx->free_lock, &lock_context); +} + +static void net_mpci_write( + net_mpci_context *ctx, + net_mpci_packet *packet, + uint32_t dest +) +{ + ssize_t n = + write(ctx->outbound_fds[dest - 1], &packet->data[0], sizeof(packet->data)); + assert(n == MAX_PACKET_SIZE); +} + +static void net_mpci_dest_to_ip_addr(uint32_t dest, char ip_addr[IP_ADDR_SIZE]) +{ + int n = snprintf(&ip_addr[0], IP_ADDR_SIZE, "10.0.1.%" PRIu32, dest); + assert(n < IP_ADDR_SIZE); +} + +static void net_mpci_outbound_server(rtems_task_argument arg) +{ + net_mpci_context *ctx = (net_mpci_context *) arg; + uint32_t n = _Objects_Maximum_nodes; + uint32_t self = _Objects_Local_node; + uint32_t dest; + + for (dest = 1; dest <= n; ++dest) { + if (dest != self) { + int fd; + char ip_addr[IP_ADDR_SIZE]; + struct sockaddr_in addr; + int ok; + int rv; + + fd = socket(PF_INET, SOCK_STREAM, 0); + assert(fd >= 0); + + net_mpci_dest_to_ip_addr(dest, ip_addr); + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(MPCI_PORT); + ok = inet_aton(&ip_addr[0], &addr.sin_addr); + assert(ok != 0); + + rv = connect(fd, (const struct sockaddr *) &addr, sizeof(addr)); + assert(rv == 0); + + ctx->outbound_fds[dest - 1] = fd; + } + } + + while (true) { + rtems_status_code sc; + net_mpci_packet *packet; + + sc = rtems_event_transient_receive(RTEMS_WAIT, RTEMS_NO_TIMEOUT); + assert(sc == RTEMS_SUCCESSFUL); + + while ((packet = net_mpci_get_outbound_packet(ctx))) { + if (packet->dest != MPCI_ALL_NODES) { + net_mpci_write(ctx, packet, packet->dest); + } else { + uint32_t n = _Objects_Maximum_nodes; + uint32_t self = _Objects_Local_node; + uint32_t dest; + + for (dest = 1; dest <= n; ++dest) { + if (dest != self) { + net_mpci_write(ctx, packet, dest); + } + } + } + + net_mpci_free_packet(ctx, packet); + } + } +} + +static void net_mpci_atexit(void) +{ + sleep(30); +} + +static void net_mpci_init(void) +{ + net_mpci_context *ctx = &net_mpci_instance; + + assert(_Objects_Maximum_nodes <= MAX_NODES); + + ctx->inbound_nfds = -1; + rtems_chain_initialize_empty(&ctx->outbound_packets); + rtems_interrupt_lock_initialize(&ctx->outbound_packets, "MPCI Outbound"); + rtems_chain_initialize( + &ctx->free_packets, + &ctx->packets[0], + PACKET_COUNT, + sizeof(ctx->packets[0]) + ); + rtems_interrupt_lock_initialize(&ctx->free_packets, "MPCI Free"); + + atexit(net_mpci_atexit); +} + +static net_mpci_packet *net_mpci_prefix_to_packet(rtems_packet_prefix *prefix) +{ + return RTEMS_CONTAINER_OF(prefix, net_mpci_packet, data); +} + +static rtems_packet_prefix *net_mpci_packet_to_prefix(net_mpci_packet *packet) +{ + return (rtems_packet_prefix *) &packet->data[0]; +} + +static net_mpci_packet *net_mpci_get_packet(net_mpci_context *ctx) +{ + rtems_interrupt_lock_context lock_context; + rtems_chain_node *node; + + rtems_interrupt_lock_acquire(&ctx->free_lock, &lock_context); + node = rtems_chain_get_unprotected(&ctx->free_packets); + rtems_interrupt_lock_release(&ctx->free_lock, &lock_context); + + return (net_mpci_packet *) node; +} + +static void net_mpci_get_prefix(rtems_packet_prefix **prefix_ptr) +{ + net_mpci_context *ctx = &net_mpci_instance; + net_mpci_packet *packet = net_mpci_get_packet(ctx); + + if (packet != NULL) { + *prefix_ptr = net_mpci_packet_to_prefix(packet); + } else { + *prefix_ptr = NULL; + } +} + +static void net_mpci_return_prefix(rtems_packet_prefix *prefix) +{ + net_mpci_context *ctx = &net_mpci_instance; + net_mpci_packet *packet = net_mpci_prefix_to_packet(prefix); + + net_mpci_free_packet(ctx, packet); +} + +static void net_mpci_send_prefix(uint32_t dest, rtems_packet_prefix *prefix) +{ + net_mpci_context *ctx = &net_mpci_instance; + net_mpci_packet *packet = net_mpci_prefix_to_packet(prefix); + rtems_status_code sc; + rtems_interrupt_lock_context lock_context; + + packet->dest = dest; + + rtems_interrupt_lock_acquire(&ctx->outbound_lock, &lock_context); + rtems_chain_append_unprotected(&ctx->outbound_packets, &packet->node); + rtems_interrupt_lock_release(&ctx->outbound_lock, &lock_context); + + sc = rtems_event_transient_send(ctx->outbound_server); + assert(sc == RTEMS_SUCCESSFUL); +} + +static void net_mpci_receive_prefix(rtems_packet_prefix **prefix_ptr) +{ + net_mpci_context *ctx = &net_mpci_instance; + uint32_t n = _Objects_Maximum_nodes - 1; + uint32_t i; + struct fd_set read_set; + int rv; + + FD_ZERO(&read_set); + + for (i = 0; i < n; ++i) { + FD_SET(ctx->inbound_fds[i], &read_set); + } + + rv = select(ctx->inbound_nfds, &read_set, NULL, NULL, NULL); + assert(rv > 0); + + for (i = 0; i < n; ++i) { + int fd = ctx->inbound_fds[i]; + + if (FD_ISSET(fd, &read_set)) { + net_mpci_packet *packet; + size_t j; + + packet = net_mpci_get_packet(ctx); + assert(packet != NULL); + + j = 0; + + do { + ssize_t n = read(fd, &packet->data[j], sizeof(packet->data) - j); + assert(n > 0); + + j += (size_t) n; + } while (j != sizeof(packet->data)); + + *prefix_ptr = net_mpci_packet_to_prefix(packet); + return; + } + } +} + +static struct rtems_bsdnet_ifconfig net_mpci_ifconfig = { + .name = RTEMS_BSP_NETWORK_DRIVER_NAME, + .attach = RTEMS_BSP_NETWORK_DRIVER_ATTACH, + .ip_address = &net_mpci_instance.ip_addr[0], + .ip_netmask = "255.255.255.0", +}; + +struct rtems_bsdnet_config rtems_bsdnet_config = { + .ifconfig = &net_mpci_ifconfig, + .network_task_priority = UINT32_MAX, + .gateway = "10.0.1.254" +}; + +__attribute__((__constructor__)) static void net_mpci_init_network(void) +{ + net_mpci_context *ctx = &net_mpci_instance; + uint32_t n = _Objects_Maximum_nodes; + uint32_t self = _Objects_Local_node; + uint32_t i; + rtems_status_code sc; + int rv; + int fd; + struct sockaddr_in addr; + + net_mpci_dest_to_ip_addr(self, ctx->ip_addr); + + rv = rtems_bsdnet_initialize_network(); + assert(rv == 0); + + sc = rtems_task_create( + rtems_build_name('M', 'P', 'C', 'I'), + 1, + RTEMS_MINIMUM_STACK_SIZE, + RTEMS_DEFAULT_ATTRIBUTES, + RTEMS_DEFAULT_MODES, + &ctx->outbound_server + ); + assert(sc == RTEMS_SUCCESSFUL); + + sc = rtems_task_start( + ctx->outbound_server, + net_mpci_outbound_server, + (rtems_task_argument) ctx + ); + assert(sc == RTEMS_SUCCESSFUL); + + fd = socket(PF_INET, SOCK_STREAM, 0); + assert(fd >= 0); + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(MPCI_PORT); + + rv = bind(fd, (struct sockaddr *) &addr, sizeof(addr)); + assert(rv == 0); + + rv = listen(fd, 0); + assert(rv == 0); + + for (i = 1; i < n; ++i) { + socklen_t addr_len; + + addr_len = sizeof(addr); + rv = accept(fd, (struct sockaddr *) &addr, &addr_len); + assert(rv >= 0); + + if (rv + 1 > ctx->inbound_nfds) { + ctx->inbound_nfds = rv + 1; + } + + ctx->inbound_fds[i - 1] = rv; + } + + rtems_multiprocessing_announce(); +} + +rtems_mpci_table net_mpci_table = { + .default_timeout = UINT32_MAX, + .maximum_packet_size = MAX_PACKET_SIZE, + .initialization = net_mpci_init, + .get_packet = net_mpci_get_prefix, + .return_packet = net_mpci_return_prefix, + .send_packet = net_mpci_send_prefix, + .receive_packet = net_mpci_receive_prefix +}; diff --git a/c/src/lib/libbsp/powerpc/shared/startup/bspstart.c b/c/src/lib/libbsp/powerpc/shared/startup/bspstart.c index 0f450fc..ca687ab 100644 --- a/c/src/lib/libbsp/powerpc/shared/startup/bspstart.c +++ b/c/src/lib/libbsp/powerpc/shared/startup/bspstart.c @@ -87,13 +87,19 @@ unsigned int BSP_time_base_divisor; void BSP_panic(char *s) { printk("%s PANIC %s\n",_RTEMS_version, s); - __asm__ __volatile ("sc"); + + while (true) { + /* Do nothing */ + } } void _BSP_Fatal_error(unsigned int v) { printk("%s PANIC ERROR %x\n",_RTEMS_version, v); - __asm__ __volatile ("sc"); + + while (true) { + /* Do nothing */ + } } /* -- 1.8.4.5 _______________________________________________ devel mailing list devel@rtems.org http://lists.rtems.org/mailman/listinfo/devel
