2727#include <mailbox.h>
2828#include <term.h>
2929
30+ // Uncomment to enable trace
31+ //#define ENABLE_TRACE 1
3032#include <trace.h>
3133
3234#include "platform_defaultatoms.h"
@@ -36,6 +38,15 @@ static uint32_t port_atom_to_gpio_port(Context *ctx, term port_atom);
3638static uint16_t gpio_port_to_rcc_port (uint32_t gpio_port );
3739static char gpio_port_to_name (uint32_t gpio_port );
3840
41+ static term create_pair (Context * ctx , term term1 , term term2 )
42+ {
43+ term ret = term_alloc_tuple (2 , ctx );
44+ term_put_tuple_element (ret , 0 , term1 );
45+ term_put_tuple_element (ret , 1 , term2 );
46+
47+ return ret ;
48+ }
49+
3950void gpiodriver_init (Context * ctx )
4051{
4152 ctx -> native_handler = consume_gpio_mailbox ;
@@ -49,17 +60,18 @@ static void consume_gpio_mailbox(Context *ctx)
4960 Message * message = mailbox_dequeue (ctx );
5061 term msg = message -> message ;
5162 term pid = term_get_tuple_element (msg , 0 );
52- term cmd = term_get_tuple_element (msg , 1 );
63+ term req = term_get_tuple_element (msg , 2 );
64+ term cmd = term_get_tuple_element (req , 0 );
5365
5466 int local_process_id = term_to_local_process_id (pid );
5567 Context * target = globalcontext_get_process (ctx -> global , local_process_id );
5668
5769 if (cmd == SET_LEVEL_ATOM ) {
58- term gpio_tuple = term_get_tuple_element (msg , 2 );
70+ term gpio_tuple = term_get_tuple_element (req , 1 );
5971 term gpio_port_atom = term_get_tuple_element (gpio_tuple , 0 );
6072 uint32_t gpio_port = port_atom_to_gpio_port (ctx , gpio_port_atom );
6173 int32_t gpio_pin_num = term_to_int32 (term_get_tuple_element (gpio_tuple , 1 ));
62- int32_t level = term_to_int32 (term_get_tuple_element (msg , 3 ));
74+ int32_t level = term_to_int32 (term_get_tuple_element (req , 2 ));
6375
6476 if (level != 0 ) {
6577 gpio_set (gpio_port , 1 << gpio_pin_num );
@@ -70,11 +82,11 @@ static void consume_gpio_mailbox(Context *ctx)
7082 ret = OK_ATOM ;
7183
7284 } else if (cmd == SET_DIRECTION_ATOM ) {
73- term gpio_tuple = term_get_tuple_element (msg , 2 );
85+ term gpio_tuple = term_get_tuple_element (req , 1 );
7486 term gpio_port_atom = term_get_tuple_element (gpio_tuple , 0 );
7587 uint32_t gpio_port = port_atom_to_gpio_port (ctx , gpio_port_atom );
7688 int32_t gpio_pin_num = term_to_int32 (term_get_tuple_element (gpio_tuple , 1 ));
77- term direction = term_get_tuple_element (msg , 3 );
89+ term direction = term_get_tuple_element (req , 2 );
7890
7991 uint16_t rcc_port = gpio_port_to_rcc_port (gpio_port );
8092 // Set direction implicitly enables the port of the GPIO
@@ -100,9 +112,16 @@ static void consume_gpio_mailbox(Context *ctx)
100112 ret = ERROR_ATOM ;
101113 }
102114
103- free (message );
115+ term ret_msg ;
116+ if (UNLIKELY (memory_ensure_free (ctx , 3 ) != MEMORY_GC_OK )) {
117+ ret_msg = OUT_OF_MEMORY_ATOM ;
118+ } else {
119+ term ref = term_get_tuple_element (msg , 1 );
120+ ret_msg = create_pair (ctx , ref , ret );
121+ }
104122
105- mailbox_send (target , ret );
123+ mailbox_send (target , ret_msg );
124+ mailbox_destroy_message (message );
106125}
107126
108127static uint32_t port_atom_to_gpio_port (Context * ctx , term port_atom )
0 commit comments