Some recovery fixes.

controller/controller/controller_bus.c

@@ -242,16 +242,22 @@ int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
 	
 	for (entry = bus_list; entry; entry = entry->next) {
 		int i;
+		int entry_error = 0;
+		int entry_recoverable = 0;
 
 		if (entry->poll) {
 			enum controller_bus_state newstate;
 			
 			newstate = entry->poll(entry);
 				
-			if (newstate == CONTROLLER_BUS_STATE_ERROR)
+			if (newstate == CONTROLLER_BUS_STATE_ERROR) {
+				entry_error++;
 				error++;
-			else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE)
+			}
+			else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
+				entry_recoverable++;
 				recoverable++;
+			}
 			else if (newstate == CONTROLLER_BUS_STATE_OK)
 				ok++;
 				
@@ -271,10 +277,13 @@ int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
 				
 				newstate = entry->input_client[i].poll(entry->input_client+i);
 				
-				if (newstate == CONTROLLER_BUS_STATE_ERROR)
+				if (newstate == CONTROLLER_BUS_STATE_ERROR) {
+					entry_error++;
 					error++;
-				else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE)
+				} else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
+					entry_recoverable++;
 					recoverable++;
+				}
 				else if (newstate == CONTROLLER_BUS_STATE_OK)
 					ok++;
 				
@@ -294,10 +303,14 @@ int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
 				
 				newstate = entry->output_client[i].poll(entry->output_client+i);
 				
-				if (newstate == CONTROLLER_BUS_STATE_ERROR)
+				if (newstate == CONTROLLER_BUS_STATE_ERROR) {
+					entry_error++;
 					error++;
-				else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE)
+				}
+				else if (newstate == CONTROLLER_BUS_STATE_RECOVERABLE) {
+					entry_recoverable++;
 					recoverable++;
+				}
 				else if (newstate == CONTROLLER_BUS_STATE_OK)
 					ok++;
 				
@@ -311,6 +324,11 @@ int controller_bus_poll_states(int *oks, int *errors, int *recoverables)
 				entry->output_client[i].state = newstate;
 			}
 		}
+		
+		if (entry_error)
+			entry->state = CONTROLLER_BUS_STATE_ERROR;
+		if (entry_recoverable)
+			entry->state = CONTROLLER_BUS_STATE_RECOVERABLE;
 	}
 	
 	if (oks)

controller/ec/block_ec.c

@@ -31,13 +31,21 @@
 
 struct controller_block_private {
 	bool pdo_data;
+	bool operational;
 };
 
 static void calculate(struct controller_block *ec)
 {
 	if (ec_rx_pdo()) {
+		ec->private->pdo_data = false;
+	} else {
 		ec->private->pdo_data = true;
 	}
+	
+	if (ec_bus->state != CONTROLLER_BUS_STATE_OK)
+		ec->private->operational = false;
+	else
+		ec->private->operational = true;
 }
 
 static void calculate_tx(struct controller_block *ec)
@@ -60,10 +68,19 @@ static struct controller_block_param_list params[] = {
 };
 
 static struct controller_block_outterm_list outterms[] = {
-	{ "pdo_data",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, pdo_data)  },
+	{ "pdo_data",     CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, pdo_data)  },
+	{ "operational",  CONTROLLER_BLOCK_TERM_BOOL, offsetof(struct controller_block_private, operational)  },
 	{ NULL },
 };
 
+static enum controller_bus_state ec_poll(struct controller_bus *bus)
+{
+	if (bus->owner->private->pdo_data)
+		return CONTROLLER_BUS_STATE_OK;
+	else
+		return CONTROLLER_BUS_STATE_ERROR;
+}
+
 struct controller_block * block_ec_create(char *name, va_list ap)
 {
 	struct controller_block *ec, *ec_tx;
@@ -113,6 +130,9 @@ struct controller_block * block_ec_create(char *name, va_list ap)
 	bus->private->logical_rx = EC_LOGICAL_RX_START;
 	bus->private->logical_tx = EC_LOGICAL_TX_START;
 
+	ec->private->pdo_data = true;
+	ec->private->operational = false;
+
 	ec->calculate = calculate;
 	ec_tx->calculate = calculate_tx;
 
@@ -128,6 +148,8 @@ struct controller_block * block_ec_create(char *name, va_list ap)
 	ec_block = ec;
 	ec_bus = bus;
 
+	controller_bus_poll_set(bus, ec_poll, NULL);
+
 	return ec;
 
 err_params:

controller/ec/esc_device.c

@@ -119,6 +119,9 @@ static enum controller_bus_state esc_device_poll(struct controller_bus_client *c
 			    state, esc_al_state2str(state));
 			dev->state = state;
 		}
+		if (state != ESC_AL_STATE_OPERATIONAL) {
+			poll_state = CONTROLLER_BUS_STATE_RECOVERABLE;
+		}
 		if (status != dev->status) {
 			if (!status) {
 				log_send(LOG_T_WARNING,
@@ -168,6 +171,8 @@ static enum controller_bus_state esc_device_poll(struct controller_bus_client *c
 			dev->status = status;
 		}
 	}
+
+	esc_al_error_ack(dev);
 	return poll_state;
 }
 
@@ -179,6 +184,7 @@ static int esc_device_recover(struct controller_bus_client *client)
 	int r;
 
 	state = esc_al_state_get(dev);
+	log_send(LOG_T_WARNING, "cur state: %d", state);
 
 	if (state == ESC_AL_STATE_ERROR) {
 		return -1;
@@ -235,6 +241,8 @@ static int esc_device_recover(struct controller_bus_client *client)
 			return -1;
 		}
 	}
+	state = esc_al_state_get(dev);
+	log_send(LOG_T_WARNING, "new state: %d", state);
 
 	return 0;
 }