From cdf872625ee1327771f67a4d3c9dbd5facb2d71e Mon Sep 17 00:00:00 2001
From: Laurens Valk <laurens@pybricks.com>
Date: Wed, 9 Dec 2020 14:43:54 +0100
Subject: [PATCH] pbdrv/ioport: run dcm if uart sensor unplugged

This fixes hotplugging of passive sensors after a UART sensor has been unplugged.

Combined with the prevous commits, this fixes #2.
---
 lib/pbio/drv/ioport/ioport_lpf2.c | 61 +++++++++++++++++++------------
 lib/pbio/src/uartdev.c            |  4 +-
 2 files changed, 40 insertions(+), 25 deletions(-)

diff --git a/lib/pbio/drv/ioport/ioport_lpf2.c b/lib/pbio/drv/ioport/ioport_lpf2.c
index aee874ad8..9a0340f60 100644
--- a/lib/pbio/drv/ioport/ioport_lpf2.c
+++ b/lib/pbio/drv/ioport/ioport_lpf2.c
@@ -481,35 +481,48 @@ PROCESS_THREAD(pbdrv_ioport_lpf2_process, ev, data) {
     }
 
     while (true) {
-        PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_TIMER && etimer_expired(&timer));
-        etimer_reset(&timer);
+        PROCESS_WAIT_EVENT();
 
-        for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) {
-            ioport_dev_t *ioport = &ioport_devs[i];
+        // If pbio_uartdev_process tells us the uart device was removed, reset
+        // ioport id so the next timer event will take care of resetting the ioport
+        if (ev == PROCESS_EVENT_SERVICE_REMOVED) {
+            for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) {
+                ioport_dev_t *ioport = &ioport_devs[i];
 
-            if (ioport->connected_type_id != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) {
-                poll_dcm(ioport);
+                if (ioport->iodev == (pbio_iodev_t *)data) {
+                    ioport->connected_type_id = PBIO_IODEV_TYPE_ID_NONE;
+                }
             }
+        } else if (ev == PROCESS_EVENT_TIMER && etimer_expired(&timer)) {
+            etimer_reset(&timer);
 
-            if (ioport->connected_type_id != ioport->prev_type_id) {
-                ioport->prev_type_id = ioport->connected_type_id;
-                if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) {
-                    ioport_enable_uart(ioport);
-                    pbio_uartdev_get(i, &ioport->iodev);
-                } else if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_NONE) {
-                    ioport->iodev = NULL;
-                } else {
-                    assert(ioport->connected_type_id < PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART);
-                    pbio_iodev_t *iodev = &basic_devs[i];
-                    const pbio_iodev_info_t *info = &basic_infos[ioport->connected_type_id].info;
-                    iodev->info = info;
-
-                    const lump_mode_flags_t *flags = &info->mode_info[0].flags;
-                    iodev->motor_flags = PBIO_IODEV_MOTOR_FLAG_NONE;
-                    if (flags->flags0 & LUMP_MODE_FLAGS0_MOTOR_POWER) {
-                        iodev->motor_flags |= PBIO_IODEV_MOTOR_FLAG_IS_MOTOR;
+            for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) {
+                ioport_dev_t *ioport = &ioport_devs[i];
+
+                if (ioport->connected_type_id != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) {
+                    poll_dcm(ioport);
+                }
+
+                if (ioport->connected_type_id != ioport->prev_type_id) {
+                    ioport->prev_type_id = ioport->connected_type_id;
+                    if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) {
+                        ioport_enable_uart(ioport);
+                        pbio_uartdev_get(i, &ioport->iodev);
+                    } else if (ioport->connected_type_id == PBIO_IODEV_TYPE_ID_NONE) {
+                        ioport->iodev = NULL;
+                    } else {
+                        assert(ioport->connected_type_id < PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART);
+                        pbio_iodev_t *iodev = &basic_devs[i];
+                        const pbio_iodev_info_t *info = &basic_infos[ioport->connected_type_id].info;
+                        iodev->info = info;
+
+                        const lump_mode_flags_t *flags = &info->mode_info[0].flags;
+                        iodev->motor_flags = PBIO_IODEV_MOTOR_FLAG_NONE;
+                        if (flags->flags0 & LUMP_MODE_FLAGS0_MOTOR_POWER) {
+                            iodev->motor_flags |= PBIO_IODEV_MOTOR_FLAG_IS_MOTOR;
+                        }
+                        ioport->iodev = iodev;
                     }
-                    ioport->iodev = iodev;
                 }
             }
         }
diff --git a/lib/pbio/src/uartdev.c b/lib/pbio/src/uartdev.c
index cf759114f..de5af403d 100644
--- a/lib/pbio/src/uartdev.c
+++ b/lib/pbio/src/uartdev.c
@@ -823,6 +823,8 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) {
 
     PT_BEGIN(&data->pt);
 
+    // TODO: wait for ioport to be ready for a uartdevice
+
     // reset state for new device
     data->info->type_id = PBIO_IODEV_TYPE_ID_NONE;
     data->iodev.motor_flags = PBIO_IODEV_MOTOR_FLAG_NONE;
@@ -1062,7 +1064,7 @@ static PT_THREAD(pbio_uartdev_update(uartdev_port_data_t * data)) {
     debug_pr("%s\n", data->last_err);
     data->err_count++;
 
-    process_post(PROCESS_BROADCAST, PROCESS_EVENT_SERVICE_REMOVED, NULL);
+    process_post(PROCESS_BROADCAST, PROCESS_EVENT_SERVICE_REMOVED, &data->iodev);
 
     PT_END(&data->pt);
 }