master/fsm_slave.c
changeset 2066 b544025bd696
parent 2045 ff2a13a4603c
child 2080 42fbd117c3e3
equal deleted inserted replaced
2065:4d8c9a441ef6 2066:b544025bd696
    55 /** Constructor.
    55 /** Constructor.
    56  */
    56  */
    57 void ec_fsm_slave_init(
    57 void ec_fsm_slave_init(
    58         ec_fsm_slave_t *fsm, /**< Slave state machine. */
    58         ec_fsm_slave_t *fsm, /**< Slave state machine. */
    59         ec_slave_t *slave, /**< EtherCAT slave. */
    59         ec_slave_t *slave, /**< EtherCAT slave. */
    60         ec_datagram_t *datagram /**< Datagram object to use. */
    60         ec_mailbox_t *mbox/**< Datagram object to use. */
    61         )
    61         )
    62 {
    62 {
    63     fsm->slave = slave;
    63     fsm->slave = slave;
    64     fsm->datagram = datagram;
    64     fsm->mbox = mbox;
    65     fsm->datagram->data_size = 0;
    65     slave->datagram.data_size = 0;
    66 
    66 
    67     EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
    67     EC_SLAVE_DBG(slave, 1, "Init FSM.\n");
    68 
    68 
    69     fsm->state = ec_fsm_slave_state_idle;
    69     fsm->state = ec_fsm_slave_state_idle;
    70 
    70 
    71     // init sub-state-machines
    71     // init sub-state-machines
    72     ec_fsm_coe_init(&fsm->fsm_coe, fsm->datagram);
    72     ec_fsm_coe_init(&fsm->fsm_coe, fsm->mbox);
    73     ec_fsm_foe_init(&fsm->fsm_foe, fsm->datagram);
    73     ec_fsm_foe_init(&fsm->fsm_foe, fsm->mbox);
    74     ec_fsm_soe_init(&fsm->fsm_soe, fsm->datagram);
    74     ec_fsm_soe_init(&fsm->fsm_soe, fsm->mbox);
    75 }
    75 }
    76 
    76 
    77 /*****************************************************************************/
    77 /*****************************************************************************/
    78 
    78 
    79 /** Destructor.
    79 /** Destructor.
    92 
    92 
    93 /** Executes the current state of the state machine.
    93 /** Executes the current state of the state machine.
    94  *
    94  *
    95  * If the state machine's datagram is not sent or received yet, the execution
    95  * If the state machine's datagram is not sent or received yet, the execution
    96  * of the state machine is delayed to the next cycle.
    96  * of the state machine is delayed to the next cycle.
    97  */
    97  *
    98 void ec_fsm_slave_exec(
    98  * \return true, if the state machine was executed
    99         ec_fsm_slave_t *fsm /**< Slave state machine. */
    99  */
   100         )
   100 int ec_fsm_slave_exec(
   101 {
   101         ec_fsm_slave_t *fsm /**< Slave state machine. */
   102     if (fsm->datagram->state == EC_DATAGRAM_SENT
   102         )
   103         || fsm->datagram->state == EC_DATAGRAM_QUEUED) {
   103 {
       
   104     if (ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_QUEUED)
       
   105         || ec_mbox_is_datagram_state(fsm->mbox,EC_DATAGRAM_SENT)) {
   104         // datagram was not sent or received yet.
   106         // datagram was not sent or received yet.
   105         return;
   107         return 0;
   106     }
   108     }
   107 
   109 
   108     fsm->state(fsm);
   110     fsm->state(fsm);
   109     return;
   111     return 1;
   110 }
   112 }
   111 
   113 
   112 /*****************************************************************************/
   114 /*****************************************************************************/
   113 
   115 
   114 /** Sets the current state of the state machine to READY
   116 /** Sets the current state of the state machine to READY
   174     // search the first external request to be processed
   176     // search the first external request to be processed
   175     list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
   177     list_for_each_entry_safe(request, next, &slave->slave_sdo_requests, list) {
   176 
   178 
   177         list_del_init(&request->list); // dequeue
   179         list_del_init(&request->list); // dequeue
   178         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   180         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   179             EC_SLAVE_WARN(slave, "Aborting SDO request,"
   181             EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
   180                     " slave has error flag set.\n");
   182                     " slave has error flag set.\n",request);
   181             request->req.state = EC_INT_REQUEST_FAILURE;
   183             request->req.state = EC_INT_REQUEST_FAILURE;
       
   184             kref_put(&request->refcount,ec_master_sdo_request_release);
   182             wake_up(&slave->sdo_queue);
   185             wake_up(&slave->sdo_queue);
   183             fsm->sdo_request = NULL;
   186             fsm->sdo_request = NULL;
   184             fsm->state = ec_fsm_slave_state_idle;
   187             fsm->state = ec_fsm_slave_state_idle;
   185             return 0;
   188             return 0;
   186         }
   189         }
   187 
   190 
   188         if (slave->current_state == EC_SLAVE_STATE_INIT) {
   191         if (slave->current_state == EC_SLAVE_STATE_INIT) {
   189             EC_SLAVE_WARN(slave, "Aborting SDO request, slave is in INIT.\n");
   192             EC_SLAVE_WARN(slave, "Aborting SDO request %p, slave is in INIT.\n",request);
   190             request->req.state = EC_INT_REQUEST_FAILURE;
   193             request->req.state = EC_INT_REQUEST_FAILURE;
       
   194             kref_put(&request->refcount,ec_master_sdo_request_release);
   191             wake_up(&slave->sdo_queue);
   195             wake_up(&slave->sdo_queue);
   192             fsm->sdo_request = NULL;
   196             fsm->sdo_request = NULL;
   193             fsm->state = ec_fsm_slave_state_idle;
   197             fsm->state = ec_fsm_slave_state_idle;
   194             return 0;
   198             return 0;
   195         }
   199         }
   196 
   200 
   197         request->req.state = EC_INT_REQUEST_BUSY;
   201         request->req.state = EC_INT_REQUEST_BUSY;
   198 
   202 
   199         // Found pending SDO request. Execute it!
   203         // Found pending SDO request. Execute it!
   200         EC_SLAVE_DBG(slave, 1, "Processing SDO request...\n");
   204         EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n",request);
   201 
   205 
   202         // Start SDO transfer
   206         // Start SDO transfer
   203         fsm->sdo_request = &request->req;
   207         fsm->sdo_request = request;
   204         fsm->state = ec_fsm_slave_state_sdo_request;
   208         fsm->state = ec_fsm_slave_state_sdo_request;
   205         ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
   209         ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
   206         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   210         ec_fsm_coe_exec(&fsm->fsm_coe); // execute immediately
   207         ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
   211         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   208         return 1;
   212         return 1;
   209     }
   213     }
   210     return 0;
   214     return 0;
   211 }
   215 }
   212 
   216 
   217 void ec_fsm_slave_state_sdo_request(
   221 void ec_fsm_slave_state_sdo_request(
   218         ec_fsm_slave_t *fsm /**< Slave state machine. */
   222         ec_fsm_slave_t *fsm /**< Slave state machine. */
   219         )
   223         )
   220 {
   224 {
   221     ec_slave_t *slave = fsm->slave;
   225     ec_slave_t *slave = fsm->slave;
   222     ec_sdo_request_t *request = fsm->sdo_request;
   226     ec_master_sdo_request_t *request = fsm->sdo_request;
   223 
   227 
   224     if (ec_fsm_coe_exec(&fsm->fsm_coe))
   228     if (ec_fsm_coe_exec(&fsm->fsm_coe))
   225     {
   229     {
   226         ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
   230         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   227         return;
   231         return;
   228     }
   232     }
   229     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
   233     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
   230         EC_SLAVE_ERR(slave, "Failed to process SDO request.\n");
   234         EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request);
   231         request->state = EC_INT_REQUEST_FAILURE;
   235         request->req.state = EC_INT_REQUEST_FAILURE;
       
   236         kref_put(&request->refcount,ec_master_sdo_request_release);
   232         wake_up(&slave->sdo_queue);
   237         wake_up(&slave->sdo_queue);
   233         fsm->sdo_request = NULL;
   238         fsm->sdo_request = NULL;
   234         fsm->state = ec_fsm_slave_state_idle;
   239         fsm->state = ec_fsm_slave_state_idle;
   235         return;
   240         return;
   236     }
   241     }
   237 
   242 
   238     EC_SLAVE_DBG(slave, 1, "Finished SDO request.\n");
   243     EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n",request);
   239 
   244 
   240     // SDO request finished
   245     // SDO request finished
   241     request->state = EC_INT_REQUEST_SUCCESS;
   246     request->req.state = EC_INT_REQUEST_SUCCESS;
       
   247     kref_put(&request->refcount,ec_master_sdo_request_release);
   242     wake_up(&slave->sdo_queue);
   248     wake_up(&slave->sdo_queue);
   243 
   249 
   244     fsm->sdo_request = NULL;
   250     fsm->sdo_request = NULL;
   245     fsm->state = ec_fsm_slave_state_ready;
   251     fsm->state = ec_fsm_slave_state_ready;
   246 }
   252 }
   259     ec_master_foe_request_t *request, *next;
   265     ec_master_foe_request_t *request, *next;
   260 
   266 
   261     // search the first request to be processed
   267     // search the first request to be processed
   262     list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
   268     list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
   263         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   269         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   264             EC_SLAVE_WARN(slave, "Aborting FOE request,"
   270             EC_SLAVE_WARN(slave, "Aborting FOE request %p,"
   265                     " slave has error flag set.\n");
   271                     " slave has error flag set.\n",request);
   266             request->req.state = EC_INT_REQUEST_FAILURE;
   272             request->req.state = EC_INT_REQUEST_FAILURE;
   267             wake_up(&slave->sdo_queue);
   273             kref_put(&request->refcount,ec_master_foe_request_release);
       
   274             wake_up(&slave->foe_queue);
   268             fsm->sdo_request = NULL;
   275             fsm->sdo_request = NULL;
   269             fsm->state = ec_fsm_slave_state_idle;
   276             fsm->state = ec_fsm_slave_state_idle;
   270             return 0;
   277             return 0;
   271         }
   278         }
   272         list_del_init(&request->list); // dequeue
   279         list_del_init(&request->list); // dequeue
   273         request->req.state = EC_INT_REQUEST_BUSY;
   280         request->req.state = EC_INT_REQUEST_BUSY;
   274 
   281 
   275         EC_SLAVE_DBG(slave, 1, "Processing FoE request.\n");
   282         EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n",request);
   276 
   283 
   277         fsm->foe_request = &request->req;
   284         fsm->foe_request = request;
   278         fsm->state = ec_fsm_slave_state_foe_request;
   285         fsm->state = ec_fsm_slave_state_foe_request;
   279         ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
   286         ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
   280         ec_fsm_foe_exec(&fsm->fsm_foe);
   287         ec_fsm_foe_exec(&fsm->fsm_foe);
   281         ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
   288         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   282         return 1;
   289         return 1;
   283     }
   290     }
   284     return 0;
   291     return 0;
   285 }
   292 }
   286 
   293 
   291 void ec_fsm_slave_state_foe_request(
   298 void ec_fsm_slave_state_foe_request(
   292         ec_fsm_slave_t *fsm /**< Slave state machine. */
   299         ec_fsm_slave_t *fsm /**< Slave state machine. */
   293         )
   300         )
   294 {
   301 {
   295     ec_slave_t *slave = fsm->slave;
   302     ec_slave_t *slave = fsm->slave;
   296     ec_foe_request_t *request = fsm->foe_request;
   303     ec_master_foe_request_t *request = fsm->foe_request;
   297 
   304 
   298     if (ec_fsm_foe_exec(&fsm->fsm_foe))
   305     if (ec_fsm_foe_exec(&fsm->fsm_foe))
   299     {
   306     {
   300         ec_master_queue_external_datagram(fsm->slave->master,fsm->datagram);
   307         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   301         return;
   308         return;
   302     }
   309     }
   303 
   310 
   304     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
   311     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
   305         EC_SLAVE_ERR(slave, "Failed to handle FoE request.\n");
   312         EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n",request);
   306         request->state = EC_INT_REQUEST_FAILURE;
   313         request->req.state = EC_INT_REQUEST_FAILURE;
       
   314         kref_put(&request->refcount,ec_master_foe_request_release);
   307         wake_up(&slave->foe_queue);
   315         wake_up(&slave->foe_queue);
   308         fsm->foe_request = NULL;
   316         fsm->foe_request = NULL;
   309         fsm->state = ec_fsm_slave_state_idle;
   317         fsm->state = ec_fsm_slave_state_idle;
   310         return;
   318         return;
   311     }
   319     }
   312 
   320 
   313     // finished transferring FoE
   321     // finished transferring FoE
   314     EC_SLAVE_DBG(slave, 1, "Successfully transferred %zu bytes of FoE"
   322     EC_SLAVE_DBG(slave, 1, "FoE request %p successfully transferred %zu bytes.\n",
   315             " data.\n", request->data_size);
   323             request,request->req.data_size);
   316 
   324 
   317     request->state = EC_INT_REQUEST_SUCCESS;
   325     request->req.state = EC_INT_REQUEST_SUCCESS;
       
   326     kref_put(&request->refcount,ec_master_foe_request_release);
   318     wake_up(&slave->foe_queue);
   327     wake_up(&slave->foe_queue);
   319 
   328 
   320     fsm->foe_request = NULL;
   329     fsm->foe_request = NULL;
   321     fsm->state = ec_fsm_slave_state_ready;
   330     fsm->state = ec_fsm_slave_state_ready;
   322 }
   331 }
   330 int ec_fsm_slave_action_process_soe(
   339 int ec_fsm_slave_action_process_soe(
   331         ec_fsm_slave_t *fsm /**< Slave state machine. */
   340         ec_fsm_slave_t *fsm /**< Slave state machine. */
   332         )
   341         )
   333 {
   342 {
   334     ec_slave_t *slave = fsm->slave;
   343     ec_slave_t *slave = fsm->slave;
   335     ec_master_soe_request_t *req, *next;
   344     ec_master_soe_request_t *request, *next;
   336 
   345 
   337     // search the first request to be processed
   346     // search the first request to be processed
   338     list_for_each_entry_safe(req, next, &slave->soe_requests, list) {
   347     list_for_each_entry_safe(request, next, &slave->soe_requests, list) {
   339 
   348 
   340         list_del_init(&req->list); // dequeue
   349         list_del_init(&request->list); // dequeue
   341         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   350         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
   342             EC_SLAVE_WARN(slave, "Aborting SoE request,"
   351             EC_SLAVE_WARN(slave, "Aborting SoE request,"
   343                     " slave has error flag set.\n");
   352                     " slave has error flag set.\n");
   344             req->req.state = EC_INT_REQUEST_FAILURE;
   353             request->req.state = EC_INT_REQUEST_FAILURE;
       
   354             kref_put(&request->refcount,ec_master_soe_request_release);
   345             wake_up(&slave->soe_queue);
   355             wake_up(&slave->soe_queue);
   346             fsm->state = ec_fsm_slave_state_idle;
   356             fsm->state = ec_fsm_slave_state_idle;
   347             return 0;
   357             return 0;
   348         }
   358         }
   349 
   359 
   350         if (slave->current_state == EC_SLAVE_STATE_INIT) {
   360         if (slave->current_state == EC_SLAVE_STATE_INIT) {
   351             EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
   361             EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
   352             req->req.state = EC_INT_REQUEST_FAILURE;
   362             request->req.state = EC_INT_REQUEST_FAILURE;
       
   363             kref_put(&request->refcount,ec_master_soe_request_release);
   353             wake_up(&slave->soe_queue);
   364             wake_up(&slave->soe_queue);
   354             fsm->state = ec_fsm_slave_state_idle;
   365             fsm->state = ec_fsm_slave_state_idle;
   355             return 0;
   366             return 0;
   356         }
   367         }
   357 
   368 
   358         req->req.state = EC_INT_REQUEST_BUSY;
   369         request->req.state = EC_INT_REQUEST_BUSY;
   359 
   370 
   360         // Found pending request. Execute it!
   371         // Found pending request. Execute it!
   361         EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
   372         EC_SLAVE_DBG(slave, 1, "Processing SoE request...\n");
   362 
   373 
   363         // Start SoE transfer
   374         // Start SoE transfer
   364         fsm->soe_request = &req->req;
   375         fsm->soe_request = request;
   365         fsm->state = ec_fsm_slave_state_soe_request;
   376         fsm->state = ec_fsm_slave_state_soe_request;
   366         ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &req->req);
   377         ec_fsm_soe_transfer(&fsm->fsm_soe, slave, &request->req);
   367         ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
   378         ec_fsm_soe_exec(&fsm->fsm_soe); // execute immediately
   368         ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
   379         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   369         return 1;
   380         return 1;
   370     }
   381     }
   371     return 0;
   382     return 0;
   372 }
   383 }
   373 
   384 
   378 void ec_fsm_slave_state_soe_request(
   389 void ec_fsm_slave_state_soe_request(
   379         ec_fsm_slave_t *fsm /**< Slave state machine. */
   390         ec_fsm_slave_t *fsm /**< Slave state machine. */
   380         )
   391         )
   381 {
   392 {
   382     ec_slave_t *slave = fsm->slave;
   393     ec_slave_t *slave = fsm->slave;
   383     ec_soe_request_t *request = fsm->soe_request;
   394     ec_master_soe_request_t *request = fsm->soe_request;
   384 
   395 
   385     if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
   396     if (ec_fsm_soe_exec(&fsm->fsm_soe)) {
   386         ec_master_queue_external_datagram(fsm->slave->master, fsm->datagram);
   397         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
   387         return;
   398         return;
   388     }
   399     }
   389 
   400 
   390     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
   401     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
   391         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
   402         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
   392         request->state = EC_INT_REQUEST_FAILURE;
   403         request->req.state = EC_INT_REQUEST_FAILURE;
       
   404         kref_put(&request->refcount,ec_master_soe_request_release);
   393         wake_up(&slave->soe_queue);
   405         wake_up(&slave->soe_queue);
   394         fsm->soe_request = NULL;
   406         fsm->soe_request = NULL;
   395         fsm->state = ec_fsm_slave_state_idle;
   407         fsm->state = ec_fsm_slave_state_idle;
   396         return;
   408         return;
   397     }
   409     }
   398 
   410 
   399     EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
   411     EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
   400 
   412 
   401     // SoE request finished
   413     // SoE request finished
   402     request->state = EC_INT_REQUEST_SUCCESS;
   414     request->req.state = EC_INT_REQUEST_SUCCESS;
       
   415     kref_put(&request->refcount,ec_master_soe_request_release);
   403     wake_up(&slave->soe_queue);
   416     wake_up(&slave->soe_queue);
   404 
   417 
   405     fsm->soe_request = NULL;
   418     fsm->soe_request = NULL;
   406     fsm->state = ec_fsm_slave_state_ready;
   419     fsm->state = ec_fsm_slave_state_ready;
   407 }
   420 }