Skip to content

Commit bc0eea1

Browse files
laurensvalklaurensvalk
committed
pbio/drv/legodev/pup_uart: Consolidate writes.
There were several write_begin and write_end calls used in a similar way. There was already a PT_THREAD for pbdrv_legodev_pup_uart_send_speed_msg, which we can generalize and use for other messages too. This saves on build size. Because write operations are all handled in the same place, we also don't need the tx_busy mutex anymore.
1 parent f43c4a6 commit bc0eea1

File tree

1 file changed

+36
-111
lines changed

1 file changed

+36
-111
lines changed

‎lib/pbio/drv/legodev/legodev_pup_uart.c‎

Lines changed: 36 additions & 111 deletions
Original file line numberDiff line numberDiff line change
@@ -257,10 +257,10 @@ typedef struct {
257257
struct _pbdrv_legodev_pup_uart_dev_t {
258258
/**< Protothread for main communication protocol. */
259259
struct pt pt;
260+
/**< Child protothread of the main protothread used for writing data */
261+
struct pt write_pt;
260262
/**< Protothread for receiving sensor data. */
261263
struct pt data_pt;
262-
/**< Protothread for setting the baud rate. */
263-
struct pt speed_pt;
264264
/**< Timer for sending keepalive messages and other delays. */
265265
struct etimer timer;
266266
/**< Device information, including mode info */
@@ -288,6 +288,8 @@ struct _pbdrv_legodev_pup_uart_dev_t {
288288
uint32_t new_baud_rate;
289289
/**< Buffer to hold messages transmitted to the device. */
290290
uint8_t *tx_msg;
291+
/**< Size of the current message being transmitted. */
292+
uint8_t tx_msg_size;
291293
/**< Buffer to hold messages received from the device. */
292294
uint8_t *rx_msg;
293295
/**< Size of the current message being received. */
@@ -300,12 +302,6 @@ struct _pbdrv_legodev_pup_uart_dev_t {
300302
uint32_t tx_start_time;
301303
/**< Flag that indicates that good DATA data->msg has been received since last watchdog timeout. */
302304
bool data_rec;
303-
/**< Mutex that protects tx_msg. */
304-
bool tx_busy;
305-
/**< Length of data to be set, data is stored in bin_data. */
306-
uint8_t data_set_len;
307-
/**< Buffer for holding baud rate change message data. */
308-
uint8_t speed_payload[4];
309305
/**< ID of the UART device to use for communications. */
310306
uint8_t uart_id;
311307
/**< data->msg to be printed in case of an error. */
@@ -743,23 +739,11 @@ static uint8_t ev3_uart_set_msg_hdr(lump_msg_type_t type, lump_msg_size_t size,
743739
return (type & LUMP_MSG_TYPE_MASK) | (size & LUMP_MSG_SIZE_MASK) | (cmd & LUMP_MSG_CMD_MASK);
744740
}
745741

746-
static pbio_error_t ev3_uart_begin_tx_msg(pbdrv_legodev_pup_uart_dev_t *port_data, lump_msg_type_t msg_type,
742+
static void ev3_uart_prepare_tx_msg(pbdrv_legodev_pup_uart_dev_t *port_data, lump_msg_type_t msg_type,
747743
lump_cmd_t cmd, const uint8_t *data, uint8_t len) {
748744
uint8_t header, checksum, i;
749745
uint8_t offset = 0;
750746
lump_msg_size_t size;
751-
pbio_error_t err;
752-
753-
if (len == 0 || len > 32) {
754-
return PBIO_ERROR_INVALID_ARG;
755-
}
756-
757-
if (port_data->tx_busy) {
758-
return PBIO_ERROR_AGAIN;
759-
}
760-
761-
port_data->tx_busy = true;
762-
port_data->tx_start_time = pbdrv_clock_get_ms();
763747

764748
if (msg_type == LUMP_MSG_TYPE_DATA) {
765749
// Only Powered Up devices support setting data, and they expect to have an
@@ -805,48 +789,21 @@ static pbio_error_t ev3_uart_begin_tx_msg(pbdrv_legodev_pup_uart_dev_t *port_dat
805789

806790
port_data->tx_msg[offset] = header;
807791
port_data->tx_msg[offset + i + 1] = checksum;
808-
809-
err = pbdrv_uart_write_begin(port_data->uart, port_data->tx_msg, offset + i + 2, EV3_UART_IO_TIMEOUT);
810-
if (err != PBIO_SUCCESS) {
811-
port_data->tx_busy = false;
812-
}
813-
814-
return err;
792+
port_data->tx_msg_size = offset + i + 2;
815793
}
816794

817-
static PT_THREAD(pbdrv_legodev_pup_uart_send_speed_msg(pbdrv_legodev_pup_uart_dev_t * data, uint32_t speed)) {
818-
pbio_error_t err;
819-
820-
PT_BEGIN(&data->speed_pt);
821-
822-
PT_WAIT_WHILE(&data->speed_pt, data->tx_busy);
823-
824-
pbio_set_uint32_le(&data->speed_payload[0], speed);
825-
PBIO_PT_WAIT_READY(&data->speed_pt, err = ev3_uart_begin_tx_msg(data,
826-
LUMP_MSG_TYPE_CMD, LUMP_CMD_SPEED,
827-
data->speed_payload, PBIO_ARRAY_SIZE(data->speed_payload)));
828-
if (err != PBIO_SUCCESS) {
829-
DBG_ERR(data->last_err = "SPEED tx begin failed");
830-
goto err;
795+
static PT_THREAD(pbdrv_legodev_pup_uart_send_prepared_msg(pbdrv_legodev_pup_uart_dev_t * data, pbio_error_t * err)) {
796+
PT_BEGIN(&data->write_pt);
797+
data->tx_start_time = pbdrv_clock_get_ms();
798+
PBIO_PT_WAIT_READY(&data->write_pt, *err = pbdrv_uart_write_begin(data->uart, data->tx_msg, data->tx_msg_size, EV3_UART_IO_TIMEOUT));
799+
if (*err == PBIO_SUCCESS) {
800+
PBIO_PT_WAIT_READY(&data->write_pt, *err = pbdrv_uart_write_end(data->uart));
831801
}
832-
833-
PBIO_PT_WAIT_READY(&data->speed_pt, err = pbdrv_uart_write_end(data->uart));
834-
if (err != PBIO_SUCCESS) {
835-
data->tx_busy = false;
836-
DBG_ERR(data->last_err = "SPEED tx end failed");
837-
goto err;
838-
}
839-
840-
data->tx_busy = false;
841-
842-
PT_END(&data->speed_pt);
843-
844-
err:
845-
PT_EXIT(&data->speed_pt);
802+
PT_END(&data->write_pt);
846803
}
847804

848805
static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * data)) {
849-
pbio_error_t err;
806+
static pbio_error_t err;
850807
uint8_t checksum;
851808

852809
PT_BEGIN(&data->pt);
@@ -855,7 +812,6 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
855812
data->device_info.type_id = PBDRV_LEGODEV_TYPE_ID_NONE;
856813
data->device_info.mode = 0;
857814
data->ext_mode = 0;
858-
data->data_set_len = 0;
859815
#if PBDRV_CONFIG_LEGODEV_MODE_INFO
860816
data->device_info.flags = PBDRV_LEGODEV_CAPABILITY_FLAG_NONE;
861817
#endif
@@ -864,9 +820,16 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
864820
debug_pr_str("Wait for STATUS_SYNCING: done\n");
865821

866822
// Send SPEED command at 115200 baud
867-
pbdrv_uart_set_baud_rate(data->uart, EV3_UART_SPEED_LPF2);
868823
debug_pr("set baud: %d\n", EV3_UART_SPEED_LPF2);
869-
PT_SPAWN(&data->pt, &data->speed_pt, pbdrv_legodev_pup_uart_send_speed_msg(data, EV3_UART_SPEED_LPF2));
824+
pbdrv_uart_set_baud_rate(data->uart, EV3_UART_SPEED_LPF2);
825+
uint8_t speed_payload[4];
826+
pbio_set_uint32_le(speed_payload, EV3_UART_SPEED_LPF2);
827+
ev3_uart_prepare_tx_msg(data, LUMP_MSG_TYPE_CMD, LUMP_CMD_SPEED, speed_payload, sizeof(speed_payload));
828+
PT_SPAWN(&data->pt, &data->write_pt, pbdrv_legodev_pup_uart_send_prepared_msg(data, &err));
829+
if (err != PBIO_SUCCESS) {
830+
DBG_ERR(data->last_err = "Speed tx failed");
831+
goto err;
832+
}
870833

871834
pbdrv_uart_flush(data->uart);
872835

@@ -993,22 +956,13 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
993956
}
994957

995958
// reply with ACK
996-
PT_WAIT_WHILE(&data->pt, data->tx_busy);
997-
data->tx_busy = true;
998959
data->tx_msg[0] = LUMP_SYS_ACK;
999-
PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_begin(data->uart, data->tx_msg, 1, EV3_UART_IO_TIMEOUT));
1000-
if (err != PBIO_SUCCESS) {
1001-
data->tx_busy = false;
1002-
DBG_ERR(data->last_err = "UART Tx begin error during ack");
1003-
goto err;
1004-
}
1005-
PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart));
960+
data->tx_msg_size = 1;
961+
PT_SPAWN(&data->pt, &data->write_pt, pbdrv_legodev_pup_uart_send_prepared_msg(data, &err));
1006962
if (err != PBIO_SUCCESS) {
1007-
data->tx_busy = false;
1008-
DBG_ERR(data->last_err = "UART Tx end error during ack");
963+
DBG_ERR(data->last_err = "UART Tx error during ack.");
1009964
goto err;
1010965
}
1011-
data->tx_busy = false;
1012966

1013967
// schedule baud rate change
1014968
etimer_set(&data->timer, 10);
@@ -1042,6 +996,7 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
1042996
// Reset other timers
1043997
etimer_reset_with_new_interval(&data->timer, EV3_UART_DATA_KEEP_ALIVE_TIMEOUT);
1044998
data->data_set->time = pbdrv_clock_get_ms();
999+
data->data_set->size = 0;
10451000

10461001
while (data->status == PBDRV_LEGODEV_PUP_UART_STATUS_DATA) {
10471002

@@ -1059,24 +1014,13 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
10591014
}
10601015
}
10611016
data->data_rec = false;
1062-
1063-
PT_WAIT_WHILE(&data->pt, data->tx_busy);
1064-
data->tx_busy = true;
10651017
data->tx_msg[0] = LUMP_SYS_NACK;
1066-
PBIO_PT_WAIT_READY(&data->pt,
1067-
err = pbdrv_uart_write_begin(data->uart, data->tx_msg, 1, EV3_UART_IO_TIMEOUT));
1018+
data->tx_msg_size = 1;
1019+
PT_SPAWN(&data->pt, &data->write_pt, pbdrv_legodev_pup_uart_send_prepared_msg(data, &err));
10681020
if (err != PBIO_SUCCESS) {
1069-
data->tx_busy = false;
1070-
DBG_ERR(data->last_err = "UART Tx begin error during keepalive");
1021+
DBG_ERR(data->last_err = "Error during keepalive.");
10711022
goto err;
10721023
}
1073-
PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart));
1074-
if (err != PBIO_SUCCESS) {
1075-
data->tx_busy = false;
1076-
DBG_ERR(data->last_err = "UART Tx end error during keepalive");
1077-
goto err;
1078-
}
1079-
data->tx_busy = false;
10801024
etimer_reset_with_new_interval(&data->timer, EV3_UART_DATA_KEEP_ALIVE_TIMEOUT);
10811025

10821026
// Retry mode switch if it hasn't been handled or failed.
@@ -1088,40 +1032,26 @@ static PT_THREAD(pbdrv_legodev_pup_uart_update(pbdrv_legodev_pup_uart_dev_t * da
10881032
// Handle requested mode change
10891033
if (data->mode_switch.requested) {
10901034
data->mode_switch.requested = false;
1091-
PBIO_PT_WAIT_READY(&data->pt, err = ev3_uart_begin_tx_msg(data, LUMP_MSG_TYPE_CMD, LUMP_CMD_SELECT, &data->mode_switch.desired_mode, 1));
1092-
if (err != PBIO_SUCCESS) {
1093-
data->tx_busy = false;
1094-
DBG_ERR(data->last_err = "Begin setting requested mode failed.");
1095-
goto err;
1096-
}
1097-
PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart));
1035+
ev3_uart_prepare_tx_msg(data, LUMP_MSG_TYPE_CMD, LUMP_CMD_SELECT, &data->mode_switch.desired_mode, 1);
1036+
PT_SPAWN(&data->pt, &data->write_pt, pbdrv_legodev_pup_uart_send_prepared_msg(data, &err));
10981037
if (err != PBIO_SUCCESS) {
1099-
data->tx_busy = false;
1100-
DBG_ERR(data->last_err = "End setting requested mode failed.");
1038+
DBG_ERR(data->last_err = "Setting requested mode failed.");
11011039
goto err;
11021040
}
1103-
data->tx_busy = false;
11041041
}
11051042

11061043
// Handle requested data set
11071044
if (data->data_set->size > 0) {
11081045
// Only set data if we are in the correct mode already.
11091046
if (data->device_info.mode == data->data_set->desired_mode) {
1110-
PBIO_PT_WAIT_READY(&data->pt, err = ev3_uart_begin_tx_msg(data, LUMP_MSG_TYPE_DATA, data->data_set->desired_mode, data->data_set->bin_data, data->data_set->size));
1047+
ev3_uart_prepare_tx_msg(data, LUMP_MSG_TYPE_DATA, data->data_set->desired_mode, data->data_set->bin_data, data->data_set->size);
11111048
data->data_set->size = 0;
11121049
data->data_set->time = pbdrv_clock_get_ms();
1050+
PT_SPAWN(&data->pt, &data->write_pt, pbdrv_legodev_pup_uart_send_prepared_msg(data, &err));
11131051
if (err != PBIO_SUCCESS) {
1114-
data->tx_busy = false;
1115-
DBG_ERR(data->last_err = "Begin setting requested data failed.");
1052+
DBG_ERR(data->last_err = "Setting requested data failed.");
11161053
goto err;
11171054
}
1118-
PBIO_PT_WAIT_READY(&data->pt, err = pbdrv_uart_write_end(data->uart));
1119-
if (err != PBIO_SUCCESS) {
1120-
data->tx_busy = false;
1121-
DBG_ERR(data->last_err = "End setting requested data failed.");
1122-
goto err;
1123-
}
1124-
data->tx_busy = false;
11251055
data->data_set->time = pbdrv_clock_get_ms();
11261056
} else if (pbdrv_clock_get_ms() - data->data_set->time < 500) {
11271057
// Not in the right mode yet, try again later for a reasonable amount of time.
@@ -1295,11 +1225,6 @@ pbio_error_t pbdrv_legodev_is_ready(pbdrv_legodev_dev_t *legodev) {
12951225
return PBIO_ERROR_AGAIN;
12961226
}
12971227

1298-
// Not ready if busy writing.
1299-
if (port_data->tx_busy) {
1300-
return PBIO_ERROR_AGAIN;
1301-
}
1302-
13031228
uint32_t time = pbdrv_clock_get_ms();
13041229

13051230
// Not ready if waiting for mode change

0 commit comments

Comments
 (0)