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 -> heap );
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,16 +60,17 @@ static NativeHandlerResult consume_gpio_mailbox(Context *ctx)
4960 Message * message = mailbox_first (& ctx -> mailbox );
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
5668 if (cmd == SET_LEVEL_ATOM ) {
57- term gpio_tuple = term_get_tuple_element (msg , 2 );
69+ term gpio_tuple = term_get_tuple_element (req , 1 );
5870 term gpio_port_atom = term_get_tuple_element (gpio_tuple , 0 );
5971 uint32_t gpio_port = port_atom_to_gpio_port (ctx , gpio_port_atom );
6072 int32_t gpio_pin_num = term_to_int32 (term_get_tuple_element (gpio_tuple , 1 ));
61- int32_t level = term_to_int32 (term_get_tuple_element (msg , 3 ));
73+ int32_t level = term_to_int32 (term_get_tuple_element (req , 2 ));
6274
6375 if (level != 0 ) {
6476 gpio_set (gpio_port , 1 << gpio_pin_num );
@@ -69,11 +81,11 @@ static NativeHandlerResult consume_gpio_mailbox(Context *ctx)
6981 ret = OK_ATOM ;
7082
7183 } else if (cmd == SET_DIRECTION_ATOM ) {
72- term gpio_tuple = term_get_tuple_element (msg , 2 );
84+ term gpio_tuple = term_get_tuple_element (req , 1 );
7385 term gpio_port_atom = term_get_tuple_element (gpio_tuple , 0 );
7486 uint32_t gpio_port = port_atom_to_gpio_port (ctx , gpio_port_atom );
7587 int32_t gpio_pin_num = term_to_int32 (term_get_tuple_element (gpio_tuple , 1 ));
76- term direction = term_get_tuple_element (msg , 3 );
88+ term direction = term_get_tuple_element (req , 2 );
7789
7890 uint16_t rcc_port = gpio_port_to_rcc_port (gpio_port );
7991 // Set direction implicitly enables the port of the GPIO
@@ -99,7 +111,14 @@ static NativeHandlerResult consume_gpio_mailbox(Context *ctx)
99111 ret = ERROR_ATOM ;
100112 }
101113
102- globalcontext_send_message (ctx -> global , local_process_id , ret );
114+ term ret_msg ;
115+ if (UNLIKELY (memory_ensure_free (ctx , 3 ) != MEMORY_GC_OK )) {
116+ ret_msg = OUT_OF_MEMORY_ATOM ;
117+ } else {
118+ term ref = term_get_tuple_element (msg , 1 );
119+ ret_msg = create_pair (ctx , ref , ret );
120+ }
121+ globalcontext_send_message (ctx -> global , local_process_id , ret_msg );
103122
104123 mailbox_remove_message (& ctx -> mailbox , & ctx -> heap );
105124
0 commit comments