--- a/master/fsm_slave_scan.c	Wed Feb 06 15:58:00 2013 +0100
+++ b/master/fsm_slave_scan.c	Wed Feb 06 17:25:08 2013 +0100
@@ -50,6 +50,9 @@
 void ec_fsm_slave_scan_state_dc_cap(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_dc_times(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *);
+#ifdef EC_SII_ASSIGN
+void ec_fsm_slave_scan_state_assign_sii(ec_fsm_slave_scan_t *);
+#endif
 void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *);
 void ec_fsm_slave_scan_state_sii_data(ec_fsm_slave_scan_t *);
 #ifdef EC_REGALIAS
@@ -439,11 +442,52 @@
 
 /*****************************************************************************/
 
+/** Enter slave scan state SII_SIZE.
+ */
+void ec_fsm_slave_scan_enter_sii_size(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
+{
+    // Start fetching SII size
+
+    fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header
+    ec_fsm_sii_read(&fsm->fsm_sii, fsm->slave, fsm->sii_offset,
+            EC_FSM_SII_USE_CONFIGURED_ADDRESS);
+    fsm->state = ec_fsm_slave_scan_state_sii_size;
+    fsm->state(fsm); // execute state immediately
+}
+
+/*****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/** Enter slave scan state ASSIGN_SII.
+ */
+void ec_fsm_slave_scan_enter_assign_sii(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+
+    // assign SII to ECAT
+    ec_datagram_fpwr(datagram, slave->station_address, 0x0500, 1);
+    EC_WRITE_U8(datagram->data, 0x00); // EtherCAT
+    fsm->retries = EC_FSM_RETRIES;
+    fsm->state = ec_fsm_slave_scan_state_assign_sii;
+}
+
+#endif
+
+/*****************************************************************************/
+
 /**
    Slave scan state: DATALINK.
 */
 
-void ec_fsm_slave_scan_state_datalink(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+void ec_fsm_slave_scan_state_datalink(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
 {
     ec_datagram_t *datagram = fsm->datagram;
     ec_slave_t *slave = fsm->slave;
@@ -478,14 +522,50 @@
             dl_status & (1 << (9 + i * 2)) ? 1 : 0;
     }
 
-    // Start fetching SII size
-
-    fsm->sii_offset = EC_FIRST_SII_CATEGORY_OFFSET; // first category header
-    ec_fsm_sii_read(&fsm->fsm_sii, slave, fsm->sii_offset,
-            EC_FSM_SII_USE_CONFIGURED_ADDRESS);
-    fsm->state = ec_fsm_slave_scan_state_sii_size;
-    fsm->state(fsm); // execute state immediately
-}
+#ifdef EC_SII_ASSIGN
+    ec_fsm_slave_scan_enter_assign_sii(fsm);
+#else
+    ec_fsm_slave_scan_enter_sii_size(fsm);
+#endif
+}
+
+/*****************************************************************************/
+
+#ifdef EC_SII_ASSIGN
+
+/**
+   Slave scan state: ASSIGN_SII.
+*/
+
+void ec_fsm_slave_scan_state_assign_sii(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
+{
+    ec_datagram_t *datagram = fsm->datagram;
+    ec_slave_t *slave = fsm->slave;
+
+    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--) {
+        return;
+    }
+
+    if (datagram->state != EC_DATAGRAM_RECEIVED) {
+        EC_SLAVE_WARN(slave, "Failed to receive SII assignment datagram: ");
+        ec_datagram_print_state(datagram);
+        // Try to go on, probably assignment is correct
+        goto continue_with_sii_size;
+    }
+
+    if (datagram->working_counter != 1) {
+        EC_SLAVE_WARN(slave, "Failed to assign SII to EtherCAT: ");
+        ec_datagram_print_wc_error(datagram);
+        // Try to go on, probably assignment is correct
+    }
+
+continue_with_sii_size:
+    ec_fsm_slave_scan_enter_sii_size(fsm);
+}
+
+#endif
 
 /*****************************************************************************/
 
@@ -493,7 +573,9 @@
    Slave scan state: SII SIZE.
 */
 
-void ec_fsm_slave_scan_state_sii_size(ec_fsm_slave_scan_t *fsm /**< slave state machine */)
+void ec_fsm_slave_scan_state_sii_size(
+        ec_fsm_slave_scan_t *fsm /**< slave state machine */
+        )
 {
     ec_slave_t *slave = fsm->slave;
     uint16_t cat_type, cat_size;