Mercurial Mercurial > etherlabmaster / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
master/fsm_slave.c
changeset 2181 cac59f7a42c4
parent 2094 83e9160319ec
child 2414 f35c7c8e6591
equal deleted inserted replaced
2176:ad4f7514ba71 2181:cac59f7a42c4 
     1 /******************************************************************************
 
     1 /******************************************************************************
 
     2  *
 
     2  *
 
     3  *  $Id$
 
     3  *  $Id: fsm_slave.c,v 83e9160319ec 2011/08/01 15:02:45 fp $
 
     4  *
 
     4  *
 
     5  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
 
     5  *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH
 
     6  *
 
     6  *
 
     7  *  This file is part of the IgH EtherCAT Master.
 
     7  *  This file is part of the IgH EtherCAT Master.
 
     8  *
 
     8  *
 
   135         ec_fsm_slave_t *fsm /**< Slave state machine. */
 
   135         ec_fsm_slave_t *fsm /**< Slave state machine. */
 
   136         )
 
   136         )
 
   137 {
 
   137 {
 
   138     // do nothing
 
   138     // do nothing
 
   139 }
 
   139 }
 
   140 
       
   141 
   140 
   142 /*****************************************************************************/
 
   141 /*****************************************************************************/
 
   143 
   142 
   144 /** Slave state: READY.
 
   143 /** Slave state: READY.
 
   145  */
 
   144  */
 
   187             fsm->state = ec_fsm_slave_state_idle;
 
   186             fsm->state = ec_fsm_slave_state_idle;
 
   188             return 0;
 
   187             return 0;
 
   189         }
 
   188         }
 
   190 
   189 
   191         if (slave->current_state == EC_SLAVE_STATE_INIT) {
 
   190         if (slave->current_state == EC_SLAVE_STATE_INIT) {
 
   192             EC_SLAVE_WARN(slave, "Aborting SDO request %p, slave is in INIT.\n",request);
 
   191             EC_SLAVE_WARN(slave, "Aborting SDO request %p,"
 
       
   192                     " slave is in INIT.\n", request);
 
   193             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);
 
   194             kref_put(&request->refcount,ec_master_sdo_request_release);
 
   195             wake_up(&slave->sdo_queue);
 
   195             wake_up(&slave->sdo_queue);
 
   196             fsm->sdo_request = NULL;
 
   196             fsm->sdo_request = NULL;
 
   197             fsm->state = ec_fsm_slave_state_idle;
 
   197             fsm->state = ec_fsm_slave_state_idle;
 
   199         }
 
   199         }
 
   200 
   200 
   201         request->req.state = EC_INT_REQUEST_BUSY;
 
   201         request->req.state = EC_INT_REQUEST_BUSY;
 
   202 
   202 
   203         // Found pending SDO request. Execute it!
 
   203         // Found pending SDO request. Execute it!
 
   204         EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n",request);
 
   204         EC_SLAVE_DBG(slave, 1, "Processing SDO request %p...\n", request);
 
   205 
   205 
   206         // Start SDO transfer
 
   206         // Start SDO transfer
 
   207         fsm->sdo_request = request;
 
   207         fsm->sdo_request = request;
 
   208         fsm->state = ec_fsm_slave_state_sdo_request;
 
   208         fsm->state = ec_fsm_slave_state_sdo_request;
 
   209         ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
 
   209         ec_fsm_coe_transfer(&fsm->fsm_coe, slave, &request->req);
 
   223         )
 
   223         )
 
   224 {
 
   224 {
 
   225     ec_slave_t *slave = fsm->slave;
 
   225     ec_slave_t *slave = fsm->slave;
 
   226     ec_master_sdo_request_t *request = fsm->sdo_request;
 
   226     ec_master_sdo_request_t *request = fsm->sdo_request;
 
   227 
   227 
   228     if (ec_fsm_coe_exec(&fsm->fsm_coe))
 
   228     if (ec_fsm_coe_exec(&fsm->fsm_coe)) {
 
   229     {
 
       
   230         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
 
   229         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
 
   231         return;
 
   230         return;
 
   232     }
 
   231     }
 
   233     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
   232     if (!ec_fsm_coe_success(&fsm->fsm_coe)) {
 
   234         EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n",request);
 
   233         EC_SLAVE_ERR(slave, "Failed to process SDO request %p.\n", request);
 
   235         request->req.state = EC_INT_REQUEST_FAILURE;
 
   234         request->req.state = EC_INT_REQUEST_FAILURE;
 
   236         kref_put(&request->refcount,ec_master_sdo_request_release);
 
   235         kref_put(&request->refcount, ec_master_sdo_request_release);
 
   237         wake_up(&slave->sdo_queue);
 
   236         wake_up(&slave->sdo_queue);
 
   238         fsm->sdo_request = NULL;
 
   237         fsm->sdo_request = NULL;
 
   239         fsm->state = ec_fsm_slave_state_idle;
 
   238         fsm->state = ec_fsm_slave_state_idle;
 
   240         return;
 
   239         return;
 
   241     }
 
   240     }
 
   242 
   241 
   243     EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n",request);
 
   242     EC_SLAVE_DBG(slave, 1, "Finished SDO request %p.\n", request);
 
   244 
   243 
   245     // SDO request finished
 
   244     // SDO request finished
 
   246     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   245     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   247     kref_put(&request->refcount,ec_master_sdo_request_release);
 
   246     kref_put(&request->refcount, ec_master_sdo_request_release);
 
   248     wake_up(&slave->sdo_queue);
 
   247     wake_up(&slave->sdo_queue);
 
   249 
   248 
   250     fsm->sdo_request = NULL;
 
   249     fsm->sdo_request = NULL;
 
   251     fsm->state = ec_fsm_slave_state_ready;
 
   250     fsm->state = ec_fsm_slave_state_ready;
 
   252 }
 
   251 }
 
   266 
   265 
   267     // search the first request to be processed
 
   266     // search the first request to be processed
 
   268     list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
 
   267     list_for_each_entry_safe(request, next, &slave->foe_requests, list) {
 
   269         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 
   268         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 
   270             EC_SLAVE_WARN(slave, "Aborting FOE request %p,"
 
   269             EC_SLAVE_WARN(slave, "Aborting FOE request %p,"
 
   271                     " slave has error flag set.\n",request);
 
   270                     " slave has error flag set.\n", request);
 
   272             request->req.state = EC_INT_REQUEST_FAILURE;
 
   271             request->req.state = EC_INT_REQUEST_FAILURE;
 
   273             kref_put(&request->refcount,ec_master_foe_request_release);
 
   272             kref_put(&request->refcount, ec_master_foe_request_release);
 
   274             wake_up(&slave->foe_queue);
 
   273             wake_up(&slave->foe_queue);
 
   275             fsm->sdo_request = NULL;
 
   274             fsm->sdo_request = NULL;
 
   276             fsm->state = ec_fsm_slave_state_idle;
 
   275             fsm->state = ec_fsm_slave_state_idle;
 
   277             return 0;
 
   276             return 0;
 
   278         }
 
   277         }
 
   279         list_del_init(&request->list); // dequeue
 
   278         list_del_init(&request->list); // dequeue
 
   280         request->req.state = EC_INT_REQUEST_BUSY;
 
   279         request->req.state = EC_INT_REQUEST_BUSY;
 
   281 
   280 
   282         EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n",request);
 
   281         EC_SLAVE_DBG(slave, 1, "Processing FoE request %p.\n", request);
 
   283 
   282 
   284         fsm->foe_request = request;
 
   283         fsm->foe_request = request;
 
   285         fsm->state = ec_fsm_slave_state_foe_request;
 
   284         fsm->state = ec_fsm_slave_state_foe_request;
 
   286         ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
 
   285         ec_fsm_foe_transfer(&fsm->fsm_foe, slave, &request->req);
 
   287         ec_fsm_foe_exec(&fsm->fsm_foe);
 
   286         ec_fsm_foe_exec(&fsm->fsm_foe);
 
   300         )
 
   299         )
 
   301 {
 
   300 {
 
   302     ec_slave_t *slave = fsm->slave;
 
   301     ec_slave_t *slave = fsm->slave;
 
   303     ec_master_foe_request_t *request = fsm->foe_request;
 
   302     ec_master_foe_request_t *request = fsm->foe_request;
 
   304 
   303 
   305     if (ec_fsm_foe_exec(&fsm->fsm_foe))
 
   304     if (ec_fsm_foe_exec(&fsm->fsm_foe)) {
 
   306     {
 
       
   307         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
 
   305         ec_slave_mbox_queue_datagrams(slave, fsm->mbox);
 
   308         return;
 
   306         return;
 
   309     }
 
   307     }
 
   310 
   308 
   311     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
 
   309     if (!ec_fsm_foe_success(&fsm->fsm_foe)) {
 
   312         EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n",request);
 
   310         EC_SLAVE_ERR(slave, "Failed to handle FoE request %p.\n", request);
 
   313         request->req.state = EC_INT_REQUEST_FAILURE;
 
   311         request->req.state = EC_INT_REQUEST_FAILURE;
 
   314         kref_put(&request->refcount,ec_master_foe_request_release);
 
   312         kref_put(&request->refcount, ec_master_foe_request_release);
 
   315         wake_up(&slave->foe_queue);
 
   313         wake_up(&slave->foe_queue);
 
   316         fsm->foe_request = NULL;
 
   314         fsm->foe_request = NULL;
 
   317         fsm->state = ec_fsm_slave_state_idle;
 
   315         fsm->state = ec_fsm_slave_state_idle;
 
   318         return;
 
   316         return;
 
   319     }
 
   317     }
 
   320 
   318 
   321     // finished transferring FoE
 
   319     // finished transferring FoE
 
   322     EC_SLAVE_DBG(slave, 1, "FoE request %p successfully transferred %zu bytes.\n",
 
   320     EC_SLAVE_DBG(slave, 1, "FoE request %p successfully"
 
   323             request,request->req.data_size);
 
   321             " transferred %zu bytes.\n", request, request->req.data_size);
 
   324 
   322 
   325     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   323     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   326     kref_put(&request->refcount,ec_master_foe_request_release);
 
   324     kref_put(&request->refcount, ec_master_foe_request_release);
 
   327     wake_up(&slave->foe_queue);
 
   325     wake_up(&slave->foe_queue);
 
   328 
   326 
   329     fsm->foe_request = NULL;
 
   327     fsm->foe_request = NULL;
 
   330     fsm->state = ec_fsm_slave_state_ready;
 
   328     fsm->state = ec_fsm_slave_state_ready;
 
   331 }
 
   329 }
 
   349         list_del_init(&request->list); // dequeue
 
   347         list_del_init(&request->list); // dequeue
 
   350         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 
   348         if (slave->current_state & EC_SLAVE_STATE_ACK_ERR) {
 
   351             EC_SLAVE_WARN(slave, "Aborting SoE request,"
 
   349             EC_SLAVE_WARN(slave, "Aborting SoE request,"
 
   352                     " slave has error flag set.\n");
 
   350                     " slave has error flag set.\n");
 
   353             request->req.state = EC_INT_REQUEST_FAILURE;
 
   351             request->req.state = EC_INT_REQUEST_FAILURE;
 
   354             kref_put(&request->refcount,ec_master_soe_request_release);
 
   352             kref_put(&request->refcount, ec_master_soe_request_release);
 
   355             wake_up(&slave->soe_queue);
 
   353             wake_up(&slave->soe_queue);
 
   356             fsm->state = ec_fsm_slave_state_idle;
 
   354             fsm->state = ec_fsm_slave_state_idle;
 
   357             return 0;
 
   355             return 0;
 
   358         }
 
   356         }
 
   359 
   357 
   360         if (slave->current_state == EC_SLAVE_STATE_INIT) {
 
   358         if (slave->current_state == EC_SLAVE_STATE_INIT) {
 
   361             EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
 
   359             EC_SLAVE_WARN(slave, "Aborting SoE request, slave is in INIT.\n");
 
   362             request->req.state = EC_INT_REQUEST_FAILURE;
 
   360             request->req.state = EC_INT_REQUEST_FAILURE;
 
   363             kref_put(&request->refcount,ec_master_soe_request_release);
 
   361             kref_put(&request->refcount, ec_master_soe_request_release);
 
   364             wake_up(&slave->soe_queue);
 
   362             wake_up(&slave->soe_queue);
 
   365             fsm->state = ec_fsm_slave_state_idle;
 
   363             fsm->state = ec_fsm_slave_state_idle;
 
   366             return 0;
 
   364             return 0;
 
   367         }
 
   365         }
 
   368 
   366 
   399     }
 
   397     }
 
   400 
   398 
   401     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
 
   399     if (!ec_fsm_soe_success(&fsm->fsm_soe)) {
 
   402         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
 
   400         EC_SLAVE_ERR(slave, "Failed to process SoE request.\n");
 
   403         request->req.state = EC_INT_REQUEST_FAILURE;
 
   401         request->req.state = EC_INT_REQUEST_FAILURE;
 
   404         kref_put(&request->refcount,ec_master_soe_request_release);
 
   402         kref_put(&request->refcount, ec_master_soe_request_release);
 
   405         wake_up(&slave->soe_queue);
 
   403         wake_up(&slave->soe_queue);
 
   406         fsm->soe_request = NULL;
 
   404         fsm->soe_request = NULL;
 
   407         fsm->state = ec_fsm_slave_state_idle;
 
   405         fsm->state = ec_fsm_slave_state_idle;
 
   408         return;
 
   406         return;
 
   409     }
 
   407     }
 
   410 
   408 
   411     EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
 
   409     EC_SLAVE_DBG(slave, 1, "Finished SoE request.\n");
 
   412 
   410 
   413     // SoE request finished
 
   411     // SoE request finished
 
   414     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   412     request->req.state = EC_INT_REQUEST_SUCCESS;
 
   415     kref_put(&request->refcount,ec_master_soe_request_release);
 
   413     kref_put(&request->refcount, ec_master_soe_request_release);
 
   416     wake_up(&slave->soe_queue);
 
   414     wake_up(&slave->soe_queue);
 
   417 
   415 
   418     fsm->soe_request = NULL;
 
   416     fsm->soe_request = NULL;
 
   419     fsm->state = ec_fsm_slave_state_ready;
 
   417     fsm->state = ec_fsm_slave_state_ready;
 
   420 }
 
   418 }
etherlabmaster