From 2a43dfdd687cac76235cba053bd559cc9a022bb1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 31 Mar 2008 18:43:24 +0200 Subject: * digital/avr/modules/host: - added mex implementation. --- digital/avr/modules/host/mex.h | 10 +- digital/avr/modules/host/mex.host.c | 481 +++++++++++++++++++++++++++++- digital/avr/modules/host/test/Makefile | 3 +- digital/avr/modules/host/test/test_mex.c | 106 +++++++ digital/avr/modules/host/test/test_mex.py | 25 ++ 5 files changed, 616 insertions(+), 9 deletions(-) create mode 100644 digital/avr/modules/host/test/test_mex.c create mode 100644 digital/avr/modules/host/test/test_mex.py (limited to 'digital/avr/modules/host') diff --git a/digital/avr/modules/host/mex.h b/digital/avr/modules/host/mex.h index a378496c..feb91188 100644 --- a/digital/avr/modules/host/mex.h +++ b/digital/avr/modules/host/mex.h @@ -81,6 +81,10 @@ mex_msg_pop_buffer (mex_msg_t *msg); int mex_msg_len (mex_msg_t *msg); +/** Get message type. */ +u8 +mex_msg_mtype (mex_msg_t *msg); + /** Connect to the mex Hub. */ void mex_node_connect (void); @@ -101,15 +105,15 @@ mex_node_wait_date (u32 date); u32 mex_node_date (void); -/** Send a message. */ +/** Send a message, msg is released. */ void mex_node_send (mex_msg_t *msg); -/** Send a request and return response. */ +/** Send a request and return response, msg is released. */ mex_msg_t * mex_node_request (mex_msg_t *msg); -/** Send a response to the currently serviced request. */ +/** Send a response to the currently serviced request, msg is released. */ void mex_node_response (mex_msg_t *msg); diff --git a/digital/avr/modules/host/mex.host.c b/digital/avr/modules/host/mex.host.c index 7e9f4362..219b23c0 100644 --- a/digital/avr/modules/host/mex.host.c +++ b/digital/avr/modules/host/mex.host.c @@ -22,9 +22,29 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#define _GNU_SOURCE #include "common.h" #include "mex.h" +#include "host.h" #include "socket.h" +#include +#include +#include +#include +#include + +/** Message header allocated size. */ +#define MEX_MSG_NEW_HEADER_SIZE 3 + +/** Message payload allocated size. */ +#define MEX_MSG_NEW_PAYLOAD_SIZE 32 + +/** Message buffer allocated size. */ +#define MEX_MSG_NEW_BUFFER_SIZE \ + (MEX_MSG_NEW_HEADER_SIZE + MEX_MSG_NEW_PAYLOAD_SIZE) + +/** Default Hub address. */ +#define MEX_DEFAULT_ADDRESS "localhost", "2442" /** Message structure. * @@ -57,16 +77,18 @@ struct mex_node_t int req; /** Table of registered handlers. */ mex_handler_t *handlers[256]; + /** Table of registered handlers user parameters. */ + void *handlers_user[256]; }; /** Create a new message from a buffer. Buffer will be free'd on message * deletion. */ static mex_msg_t * -mex_msg_new_buffer (u8 *buffer, int size); +mex_msg_new_buffer (u8 *buf, int size); /** Encapsulate a message and fill header. */ static void -mex_msg_encapsulate (mex_msg_t *msg, const char *fmt, ...); +mex_msg_encapsulate (mex_msg_t *msg, u8 mtype, const char *fmt, ...); /** Decapsulate a message, the current payload becomes the full message. */ static void @@ -82,12 +104,461 @@ mex_node_dispatch (mex_msg_t *msg); /** Handle an incoming DATE. */ static void -mex_node_handle_DATE (mex_msg_t *msg); +mex_node_handle_DATE (void *user, mex_msg_t *msg); /** Handle an incoming REQ. */ static void -mex_node_handle_REQ (mex_msg_t *msg); +mex_node_handle_REQ (void *user, mex_msg_t *msg); + /** Global context. */ -static struct mex_node_t mex_node_global; +static struct mex_node_t mex_node_global = { .socket = -1, .req = -1 }; + + +/** Create a new message with the given type. */ +mex_msg_t * +mex_msg_new (u8 mtype) +{ + mex_msg_t *msg = malloc (sizeof (mex_msg_t)); + assert (msg); + msg->mtype = mtype; + /* Allocate buffer and fill header. */ + u8 *buf = malloc (MEX_MSG_NEW_BUFFER_SIZE); + assert (buf); + msg->buffer_head = buf; + msg->buffer_tail = buf + MEX_MSG_NEW_BUFFER_SIZE; + u8 *header = buf + MEX_MSG_NEW_HEADER_SIZE - 1; + msg->header_head = header; + msg->header_tail = header + 1; + *header = mtype; + msg->payload_head = msg->header_tail; + msg->payload_tail = msg->payload_head; + return msg; +} + +/** Create a new message from a buffer. Buffer will be free'd on message + * deletion. */ +static mex_msg_t * +mex_msg_new_buffer (u8 *buf, int size) +{ + assert (size >= 1); + mex_msg_t *msg = malloc (sizeof (mex_msg_t)); + assert (msg); + /* Decode header. */ + msg->mtype = buf[0]; + /* Position pointers. */ + msg->buffer_head = buf; + msg->buffer_tail = buf + size; + msg->header_head = buf; + msg->header_tail = buf + 1; + msg->payload_head = buf + 1; + msg->payload_tail = buf + size; + return msg; +} + +/** Release a message. */ +void +mex_msg_delete (mex_msg_t *msg) +{ + free (msg->buffer_head); + free (msg); +} + +/** Add data to the message payload. + * + * The fmt string describes the provided data which must follow: + * - b: 8 bits. + * - h: 16 bits. + * - l: 32 bits. + * + * Uppercase is used for unsigned (but who cares?). */ +void +mex_msg_push (mex_msg_t *msg, const char *fmt, ...) +{ + const char *p; + va_list a; + va_start (a, fmt); + for (p = fmt; *p; p++) + { + /* Due to promotion, all types are int (on platforms with 32 bit and + * more integers). */ + unsigned int v = va_arg (a, unsigned int); + switch (*p) + { + case 'l': + case 'L': + assert (msg->payload_tail + 4 <= msg->buffer_tail); + *msg->payload_tail++ = v >> 24; + *msg->payload_tail++ = v >> 16; + /* no break */ + case 'h': + case 'H': + assert (msg->payload_tail + 2 <= msg->buffer_tail); + *msg->payload_tail++ = v >> 8; + /* no break */ + case 'b': + case 'B': + assert (msg->payload_tail + 1 <= msg->buffer_tail); + *msg->payload_tail++ = v >> 0; + break; + default: + assert (0); + } + } + va_end (a); +} + +/** Add data to the message payload, using a buffer. */ +void +mex_msg_push_buffer (mex_msg_t *msg, const u8 *buf, int size) +{ + assert (size > 0 && msg->payload_tail + size <= msg->buffer_tail); + memcpy (msg->payload_tail, buf, size); + msg->payload_tail += size; +} + +/** Get data from the message payload. + * + * The fmt string follows the same syntax as mex_msg_push, but pointers are + * provided as extra arguments. */ +void +mex_msg_pop (mex_msg_t *msg, const char *fmt, ...) +{ + const char *p; + va_list a; + va_start (a, fmt); + u32 v; + u32 *p32; + u16 *p16; + u8 *p8; + for (p = fmt; *p; p++) + { + const u8 *h = msg->payload_head; + switch (*p) + { + case 'l': + case 'L': + assert (h + 4 <= msg->payload_tail); + v = (h[0] << 24) | (h[1] << 16) | (h[2] << 8) | (h[3] << 0); + p32 = va_arg (a, u32 *); + *p32 = v; + msg->payload_head += 4; + break; + case 'h': + case 'H': + assert (h + 2 <= msg->payload_tail); + v = (h[0] << 8) | (h[1] << 0); + p16 = va_arg (a, u16 *); + *p16 = v; + msg->payload_head += 2; + break; + case 'b': + case 'B': + assert (h + 1 <= msg->payload_tail); + v = (h[0] << 0); + p8 = va_arg (a, u8 *); + *p8 = v; + msg->payload_head += 1; + break; + default: + assert (0); + } + } + va_end (a); +} + +/** Get remaining payload from the message. */ +const u8 * +mex_msg_pop_buffer (mex_msg_t *msg) +{ + return msg->payload_head; +} + +/** Get payload remaining length. */ +int +mex_msg_len (mex_msg_t *msg) +{ + return msg->payload_tail - msg->payload_head; +} + +/** Get message type. */ +u8 +mex_msg_mtype (mex_msg_t *msg) +{ + return msg->mtype; +} + +/** Encapsulate a message and fill header. */ +static void +mex_msg_encapsulate (mex_msg_t *msg, u8 mtype, const char *fmt, ...) +{ + assert (msg->header_tail == msg->payload_head); + msg->payload_head = msg->header_head; + /* Count new header size. */ + const char *p; + int hsize = 1; + for (p = fmt; *p; p++) + { + switch (*p) + { + case 'l': + case 'L': + hsize += 4; + break; + case 'h': + case 'H': + hsize += 2; + break; + case 'b': + case 'B': + hsize += 1; + break; + } + } + msg->header_head -= hsize; + msg->header_tail = msg->header_head; + assert (hsize >= 0 && msg->header_head >= msg->buffer_head); + /* Message type. */ + *msg->header_tail++ = mtype; + /* Fill header. */ + va_list a; + va_start (a, fmt); + for (p = fmt; *p; p++) + { + /* Due to promotion, all types are int (on platforms with 32 bit and + * more integers). */ + unsigned int v = va_arg (a, unsigned int); + switch (*p) + { + case 'l': + case 'L': + *msg->header_tail++ = v >> 24; + *msg->header_tail++ = v >> 16; + /* no break */ + case 'h': + case 'H': + *msg->header_tail++ = v >> 8; + /* no break */ + case 'b': + case 'B': + *msg->header_tail++ = v >> 0; + break; + default: + assert (0); + } + } + assert (msg->header_tail == msg->payload_head); +} + +/** Decapsulate a message, the current payload becomes the full message. */ +static void +mex_msg_decapsulate (mex_msg_t *msg) +{ + assert (msg->payload_head + 1 <= msg->payload_tail); + msg->header_head = msg->payload_head; + msg->header_tail = msg->payload_head = msg->header_head + 1; + msg->mtype = msg->header_head[0]; +} + +/** Connect to the mex Hub. */ +void +mex_node_connect (void) +{ + assert (mex_node_global.socket == -1); + /* Try to reuse an already opened connection. */ + mex_node_global.socket = host_fetch_integer ("mex_node_socket"); + if (mex_node_global.socket == -1) + { + /* Connect. */ + mex_node_global.socket = socket_client (MEX_DEFAULT_ADDRESS); + host_register_integer ("mex_node_socket", mex_node_global.socket); + } + else + { + /* Request synchronisation. */ + mex_msg_t *m = mex_msg_new (MEX_MTYPE_DATE); + mex_node_send (m); + } + /* Setup default handlers. */ + mex_node_register (MEX_MTYPE_DATE, mex_node_handle_DATE, NULL); + mex_node_register (MEX_MTYPE_REQ, mex_node_handle_REQ, NULL); + /* Synchronise with the Hub. */ + int sync = 0; + while (!sync) + { + mex_msg_t *m = mex_node_recv (); + if (m->mtype == MEX_MTYPE_DATE) + sync = 1; + mex_node_dispatch (m); + mex_msg_delete (m); + } +} + +/** Close connection. */ +void +mex_node_close (void) +{ + assert (mex_node_global.socket != -1); + close (mex_node_global.socket); + mex_node_global.socket = -1; + mex_node_global.req = -1; +} + +/** Wait forever. */ +void +mex_node_wait (void) +{ + mex_node_wait_date ((u32) -1); +} + +/** Wait until a date is reached. */ +void +mex_node_wait_date (u32 date) +{ + while (date == (u32) -1 || mex_node_global.date != date) + { + /* Signal IDLE mode. */ + mex_msg_t *idle = mex_msg_new (MEX_MTYPE_IDLE); + if (date != (u32) -1) + mex_msg_push (idle, "L", date); + mex_node_send (idle); + /* Receive and dispatch message. */ + mex_msg_t *m = mex_node_recv (); + mex_node_dispatch (m); + mex_msg_delete (m); + } +} + +/** Return current date. */ +u32 +mex_node_date (void) +{ + return mex_node_global.date; +} + +/** Send a message, msg is released. */ +void +mex_node_send (mex_msg_t *msg) +{ + assert (mex_node_global.socket != -1); + assert (msg->header_tail == msg->payload_head); + int size = msg->payload_tail - msg->header_head; + int r; + u8 header[3]; + header[0] = size >> 8; + header[1] = size >> 0; + header[2] = mex_node_global.seq; + r = write (mex_node_global.socket, header, sizeof (header)); + if (r == -1) + assert_perror (errno); + assert (r == sizeof (header)); + r = write (mex_node_global.socket, msg->header_head, size); + if (r == -1) + assert_perror (errno); + assert (r == size); + mex_msg_delete (msg); +} + +/** Send a request and return response, msg is released. */ +mex_msg_t * +mex_node_request (mex_msg_t *msg) +{ + /* Send request. */ + mex_msg_encapsulate (msg, MEX_MTYPE_REQ, "B", 0); + mex_node_send (msg); + /* Wait for response. */ + mex_msg_t *rsp; + rsp = mex_node_recv (); + while (rsp->mtype != MEX_MTYPE_RSP) + { + mex_node_dispatch (rsp); + mex_msg_delete (rsp); + rsp = mex_node_recv (); + } + /* Eat unused reqid. */ + u8 reqid; + mex_msg_pop (rsp, "B", &reqid); + mex_msg_decapsulate (rsp); + return rsp; +} + +/** Send a response to the currently serviced request, msg is released. */ +void +mex_node_response (mex_msg_t *msg) +{ + assert (mex_node_global.req != -1); + mex_msg_encapsulate (msg, MEX_MTYPE_RSP, "B", mex_node_global.req); + mex_node_send (msg); + mex_node_global.req = -1; +} + +/** Register a handler for the given message type. */ +void +mex_node_register (u8 mtype, mex_handler_t *handler, void *user) +{ + assert (mex_node_global.handlers[mtype] == NULL); + mex_node_global.handlers[mtype] = handler; + mex_node_global.handlers_user[mtype] = user; +} + +/** Receive one message. */ +static mex_msg_t * +mex_node_recv (void) +{ + int r; + assert (mex_node_global.socket != -1); + /* Read header. */ + u8 header[3]; + r = read (mex_node_global.socket, header, sizeof (header)); + if (r == -1) + assert_perror (errno); + else if (r == 0) + { + mex_node_close (); + exit (0); + } + /* Decode header. */ + assert (r == sizeof (header)); + int size = (header[0] << 8) | header[1]; + mex_node_global.seq = header[2]; + /* Read whole message. */ + u8 *buf = malloc (size); + assert (buf); + r = read (mex_node_global.socket, buf, size); + if (r == -1) + assert_perror (errno); + assert (r == size); + mex_msg_t *msg = mex_msg_new_buffer (buf, size); + return msg; +} + +/** Call the right handler for the given message. */ +static void +mex_node_dispatch (mex_msg_t *msg) +{ + if (mex_node_global.handlers[msg->mtype]) + { + mex_node_global.handlers[msg->mtype] + (mex_node_global.handlers_user[msg->mtype], msg); + } +} + +/** Handle an incoming DATE. */ +static void +mex_node_handle_DATE (void *user, mex_msg_t *msg) +{ + mex_msg_pop (msg, "L", &mex_node_global.date); +} + +/** Handle an incoming REQ. */ +static void +mex_node_handle_REQ (void *user, mex_msg_t *msg) +{ + u8 req; + mex_msg_pop (msg, "B", &req); + mex_node_global.req = req; + mex_msg_decapsulate (msg); + mex_node_dispatch (msg); + mex_node_global.req = -1; +} diff --git a/digital/avr/modules/host/test/Makefile b/digital/avr/modules/host/test/Makefile index 5f8343c4..aca134a5 100644 --- a/digital/avr/modules/host/test/Makefile +++ b/digital/avr/modules/host/test/Makefile @@ -1,6 +1,7 @@ BASE = ../../.. -HOST_PROGS = test_host +HOST_PROGS = test_host test_mex test_host_SOURCES = test_host.c +test_mex_SOURCES = test_mex.c MODULES = host CONFIGFILE = avrconfig.h # atmega8, atmega8535, atmega128... diff --git a/digital/avr/modules/host/test/test_mex.c b/digital/avr/modules/host/test/test_mex.c new file mode 100644 index 00000000..1e5d303e --- /dev/null +++ b/digital/avr/modules/host/test/test_mex.c @@ -0,0 +1,106 @@ +/* test_mex.c */ +/* avr.host - Host fonctions modules. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "modules/host/host.h" +#include "modules/host/mex.h" + +#include + +void +a82 (void *user, mex_msg_t *msg) +{ + printf ("oucouc\n"); + u8 nb; + mex_msg_pop (msg, "B", &nb); + nb++; + mex_msg_t *m = mex_msg_new (mex_msg_mtype (msg)); + mex_msg_push (m, "B", nb); + mex_node_response (m); +} + +void +a801 (void *user, mex_msg_t *msg) +{ + printf ("coucou\n"); + u8 b; + u16 h; + u32 l; + mex_msg_pop (msg, "BHL", &b, &h, &l); + if (mex_msg_mtype (msg) == 0x80) + assert (b == 1 && h == 2 && l == 3); + else + assert (b == 4 && h == 5 && l == 6); +} + +int +main (int argc, char **argv) +{ + int i; + printf ("reset\n"); + host_init (argc, argv); + if (argc != 2 || !((argv[1][0] == '1' || argv[1][0] == '2') + && argv[1][1] == '\0')) + { + fprintf (stderr, "%s 1|2\n", argv[0]); + return 1; + } + if (argv[1][0] == '1') + { + mex_node_register (0x82, a82, NULL); + mex_node_connect (); + i = host_fetch_integer ("reseted"); + if (i == -1) + { + mex_msg_t *m = mex_msg_new (0x80); + mex_msg_push (m, "BHL", 1, 2, 3); + mex_node_send (m); + host_register_integer ("reseted", 1); + host_reset (); + } + else + { + mex_msg_t *m = mex_msg_new (0x81); + mex_msg_push (m, "BHL", 4, 5, 6); + mex_node_send (m); + mex_node_wait (); + } + } + else + { + mex_node_register (0x80, a801, NULL); + mex_node_register (0x81, a801, NULL); + mex_node_connect (); + mex_msg_t *m = mex_msg_new (0x82); + mex_msg_push (m, "B", 42); + mex_msg_t *r = mex_node_request (m); + assert (mex_msg_mtype (r) == 0x82); + u8 rb; + mex_msg_pop (r, "B", &rb); + assert (rb == 43); + mex_node_wait_date (42); + mex_node_wait (); + } + return 0; +} diff --git a/digital/avr/modules/host/test/test_mex.py b/digital/avr/modules/host/test/test_mex.py new file mode 100644 index 00000000..7de5555d --- /dev/null +++ b/digital/avr/modules/host/test/test_mex.py @@ -0,0 +1,25 @@ +import sys +sys.path.append (sys.path[0] + '/../../../../../host/mex') + +from mex.hub import Hub +from mex.msg import Msg + +import os, signal + +def log (x): + print x + +h = Hub (min_clients = 2, log = log) + +pid1 = os.spawnl (os.P_NOWAIT, './test_mex.host', './test_mex.host', '1') +pid2 = os.spawnl (os.P_NOWAIT, './test_mex.host', './test_mex.host', '2') + +try: + h.wait () +finally: + os.kill (pid1, signal.SIGTERM) + os.waitpid (pid1, 0) + os.kill (pid2, signal.SIGTERM) + os.waitpid (pid2, 0) + import time + time.sleep (1) -- cgit v1.2.3