diff --git a/include/firmata/firmata.h b/include/firmata/firmata.h index 72961cb..4ef4bf0 100644 --- a/include/firmata/firmata.h +++ b/include/firmata/firmata.h @@ -24,6 +24,8 @@ #pragma once +#include + #include "uart.h" #define MODE_INPUT 0x00 @@ -99,6 +101,7 @@ typedef struct s_firmata { char firmware[140]; uint8_t dev_count; struct _firmata** devs; + pthread_spinlock_t lock; } t_firmata; t_firmata* firmata_new(const char* name); diff --git a/src/firmata/firmata.c b/src/firmata/firmata.c index 67355b5..68068f3 100644 --- a/src/firmata/firmata.c +++ b/src/firmata/firmata.c @@ -39,6 +39,13 @@ firmata_new(const char* name) return NULL; } + int ret = pthread_spin_init(&res->lock, PTHREAD_PROCESS_SHARED); + if (ret != 0) { + syslog(LOG_ERR, "firmata; could not init locking"); + free(res); + return NULL; + } + res->uart = mraa_uart_init_raw(name); if (res->uart == NULL) { syslog(LOG_ERR, "firmata: UART failed to setup"); @@ -133,7 +140,9 @@ firmata_endParse(t_firmata* firmata) int analog_val = firmata->parse_buff[1] | (firmata->parse_buff[2] << 7); for (pin = 0; pin < 128; pin++) { if (firmata->pins[pin].analog_channel == analog_ch) { + if (pthread_spin_lock(&firmata->lock) != 0) return; firmata->pins[pin].value = analog_val; + if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock"); return; } } @@ -147,7 +156,9 @@ firmata_endParse(t_firmata* firmata) for (mask = 1; mask & 0xFF; mask <<= 1, pin++) { if (firmata->pins[pin].mode == MODE_INPUT) { uint32_t val = (port_val & mask) ? 1 : 0; + if (pthread_spin_lock(&firmata->lock)) return; firmata->pins[pin].value = val; + if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock"); } } return; @@ -240,10 +251,12 @@ firmata_endParse(t_firmata* firmata) int reg = (firmata->parse_buff[4] & 0x7f) | ((firmata->parse_buff[5] & 0x7f) << 7); int i = 6; int ii = 0; + if (pthread_spin_lock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock, skipping i2c msg"); for (; ii < (firmata->parse_count - 7) / 2; ii++) { firmata->i2cmsg[addr][reg+ii] = (firmata->parse_buff[i] & 0x7f) | ((firmata->parse_buff[i+1] & 0x7f) << 7); i = i+2; } + if (pthread_spin_unlock(&firmata->lock) != 0) syslog(LOG_ERR, "firmata: Fatal spinlock deadlock"); } else { if (firmata->devs != NULL) { struct _firmata* devs = firmata->devs[0]; diff --git a/src/firmata/firmata_mraa.c b/src/firmata/firmata_mraa.c index f515972..e370824 100644 --- a/src/firmata/firmata_mraa.c +++ b/src/firmata/firmata_mraa.c @@ -84,6 +84,7 @@ mraa_result_t mraa_firmata_close(mraa_firmata_context dev) { mraa_firmata_response_stop(dev); + pthread_spin_destroy(&firmata_dev->lock); free(dev); return MRAA_SUCCESS; } @@ -182,11 +183,17 @@ static mraa_result_t mraa_firmata_i2c_wait(int addr, int reg) { int i = 0; - for (; firmata_dev->i2cmsg[addr][reg] == -1; i++) { + if (pthread_spin_lock(&firmata_dev->lock) != 0) return MRAA_ERROR_UNSPECIFIED; + int res = firmata_dev->i2cmsg[addr][reg]; + if (pthread_spin_unlock(&firmata_dev->lock) != 0) return MRAA_ERROR_UNSPECIFIED; + for (; res == -1; i++) { if (i > 50) { return MRAA_ERROR_UNSPECIFIED; } usleep(500); + if (pthread_spin_lock(&firmata_dev->lock) != 0) return MRAA_ERROR_UNSPECIFIED; + res = firmata_dev->i2cmsg[addr][reg]; + if (pthread_spin_unlock(&firmata_dev->lock) != 0) return MRAA_ERROR_UNSPECIFIED; } return MRAA_SUCCESS; } @@ -345,7 +352,10 @@ mraa_firmata_aio_read(mraa_aio_context dev) { // careful, whilst you need to enable '0' for A0 you then need to read 14 // in t_firmata because well that makes sense doesn't it... - return (int) firmata_dev->pins[dev->channel].value; + if (pthread_spin_lock(&firmata_dev->lock) != 0) return -1; + int ret = (int) firmata_dev->pins[dev->channel].value; + if (pthread_spin_unlock(&firmata_dev->lock) != 0) return -1; + return ret; } static mraa_result_t @@ -380,7 +390,10 @@ mraa_firmata_gpio_mode_replace(mraa_gpio_context dev, mraa_gpio_mode_t mode) static int mraa_firmata_gpio_read_replace(mraa_gpio_context dev) { - return firmata_dev->pins[dev->pin].value; + if (pthread_spin_lock(&firmata_dev->lock) != 0) return -1; + int res = firmata_dev->pins[dev->pin].value; + if (pthread_spin_unlock(&firmata_dev->lock) != 0) return -1; + return res; } static mraa_result_t