58 void ec_fsm_master_sdo_request(ec_fsm_t *);

   113 void ec_fsm_coe_up_start(ec_fsm_t *);

   114 void ec_fsm_coe_up_request(ec_fsm_t *);

   115 void ec_fsm_coe_up_check(ec_fsm_t *);

   116 void ec_fsm_coe_up_response(ec_fsm_t *);

   117 void ec_fsm_coe_up_seg_request(ec_fsm_t *);

   118 void ec_fsm_coe_up_seg_check(ec_fsm_t *);

   119 void ec_fsm_coe_up_seg_response(ec_fsm_t *);

   120 

   580     ec_sdo_request_t *request, *next_request;

   616 

   617     // check, if there are pending SDO requests

   618     list_for_each_entry_safe(request, next_request,

   619                              &master->sdo_requests, queue) {

   620         // TODO: critical section!

   621         list_del_init(&request->queue);

   622 

   623         slave = request->sdo->slave;

   624 

   625         if (slave->current_state == EC_SLAVE_STATE_INIT

   626             || !slave->online

   627             || slave->error_flag) {

   628             request->return_code = -1;

   629             wake_up_interruptible(&master->sdo_wait_queue);

   630             continue;

   631         }

   632 

   633         // start uploading SDO

   634         fsm->slave = slave;

   635         fsm->master_state = ec_fsm_master_sdo_request;

   636         fsm->coe_request = request;

   637         fsm->coe_state = ec_fsm_coe_up_start;

   638         fsm->coe_state(fsm); // execute immediately

   639         return;

   640     }

  1087 

  1088     // restart master state machine.

  1089     fsm->master_state = ec_fsm_master_start;

  1090     fsm->master_state(fsm); // execute immediately

  1091 }

  1092 

  1093 /*****************************************************************************/

  1094 

  1095 /**

  1096    Master state: SDO REQUEST.

  1097 */

  1098 

  1099 void ec_fsm_master_sdo_request(ec_fsm_t *fsm /**< finite state machine */)

  1100 {

  1101     ec_master_t *master = fsm->master;

  1102     ec_sdo_request_t *request = fsm->coe_request;

  1103 

  1104     fsm->coe_state(fsm); // execute CoE state machine

  1105 

  1106     if (fsm->coe_state == ec_fsm_error) {

  1107         request->return_code = -1;

  1108         wake_up_interruptible(&master->sdo_wait_queue);

  1109         fsm->master_state = ec_fsm_master_start;

  1110         fsm->master_state(fsm); // execute immediately

  1111         return;

  1112     }

  1113 

  1114     if (fsm->coe_state != ec_fsm_end) return;

  1115 

  1116     // SDO dictionary fetching finished

  1117 

  1118     request->return_code = 1;

  1119     wake_up_interruptible(&master->sdo_wait_queue);

  3053         return;

  3054     }

  3055 

  3056     fsm->coe_state = ec_fsm_end; // success

  3057 }

  3058 

  3059 /*****************************************************************************/

  3060 

  3061 /**

  3062    CoE state: UP START.

  3063 */

  3064 

  3065 void ec_fsm_coe_up_start(ec_fsm_t *fsm /**< finite state machine */)

  3066 {

  3067     ec_datagram_t *datagram = &fsm->datagram;

  3068     ec_slave_t *slave = fsm->slave;

  3069     ec_master_t *master = slave->master;

  3070     ec_sdo_request_t *request = fsm->coe_request;

  3071     ec_sdo_t *sdo = request->sdo;

  3072     ec_sdo_entry_t *entry = request->entry;

  3073     uint8_t *data;

  3074 

  3075     EC_INFO("Uploading SDO 0x%04X:%i from slave %i.\n",

  3076             sdo->index, entry->subindex, slave->ring_position);

  3077 

  3078     if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 6))) {

  3079         fsm->coe_state = ec_fsm_error;

  3080         return;

  3081     }

  3082 

  3083     EC_WRITE_U16(data, 0x2 << 12); // SDO request

  3084     EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request

  3085     EC_WRITE_U16(data + 3, sdo->index);

  3086     EC_WRITE_U8 (data + 5, entry->subindex);

  3087 

  3088     if (master->debug_level) {

  3089         EC_DBG("Upload request:\n");

  3090         ec_print_data(data, 6);

  3091     }

  3092 

  3093     ec_master_queue_datagram(fsm->master, datagram);

  3094     fsm->coe_state = ec_fsm_coe_up_request;

  3095 }

  3096 

  3097 /*****************************************************************************/

  3098 

  3099 /**

  3100    CoE state: UP REQUEST.

  3101 */

  3102 

  3103 void ec_fsm_coe_up_request(ec_fsm_t *fsm /**< finite state machine */)

  3104 {

  3105     ec_datagram_t *datagram = &fsm->datagram;

  3106     ec_slave_t *slave = fsm->slave;

  3107 

  3108     if (datagram->state != EC_DATAGRAM_RECEIVED

  3109         || datagram->working_counter != 1) {

  3110         fsm->coe_state = ec_fsm_error;

  3111         EC_ERR("Reception of CoE upload request failed.\n");

  3112         return;

  3113     }

  3114 

  3115     fsm->coe_start = datagram->cycles_sent;

  3116 

  3117     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  3118     ec_master_queue_datagram(fsm->master, datagram);

  3119     fsm->coe_state = ec_fsm_coe_up_check;

  3120 }

  3121 

  3122 /*****************************************************************************/

  3123 

  3124 /**

  3125    CoE state: UP CHECK.

  3126 */

  3127 

  3128 void ec_fsm_coe_up_check(ec_fsm_t *fsm /**< finite state machine */)

  3129 {

  3130     ec_datagram_t *datagram = &fsm->datagram;

  3131     ec_slave_t *slave = fsm->slave;

  3132 

  3133     if (datagram->state != EC_DATAGRAM_RECEIVED

  3134         || datagram->working_counter != 1) {

  3135         fsm->coe_state = ec_fsm_error;

  3136         EC_ERR("Reception of CoE mailbox check datagram failed.\n");

  3137         return;

  3138     }

  3139 

  3140     if (!ec_slave_mbox_check(datagram)) {

  3141         if (datagram->cycles_received

  3142             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {

  3143             fsm->coe_state = ec_fsm_error;

  3144             EC_ERR("Timeout while checking SDO upload on slave %i.\n",

  3145                    slave->ring_position);

  3146             return;

  3147         }

  3148 

  3149         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  3150         ec_master_queue_datagram(fsm->master, datagram);

  3151         return;

  3152     }

  3153 

  3154     // Fetch response

  3155     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

  3156     ec_master_queue_datagram(fsm->master, datagram);

  3157     fsm->coe_state = ec_fsm_coe_up_response;

  3158 }

  3159 

  3160 /*****************************************************************************/

  3161 

  3162 /**

  3163    CoE state: UP RESPONSE.

  3164 */

  3165 

  3166 void ec_fsm_coe_up_response(ec_fsm_t *fsm /**< finite state machine */)

  3167 {

  3168     ec_datagram_t *datagram = &fsm->datagram;

  3169     ec_slave_t *slave = fsm->slave;

  3170     ec_master_t *master = slave->master;

  3171     uint8_t *data, mbox_prot;

  3172     size_t rec_size, data_size;

  3173     ec_sdo_request_t *request = fsm->coe_request;

  3174     ec_sdo_t *sdo = request->sdo;

  3175     ec_sdo_entry_t *entry = request->entry;

  3176     uint32_t complete_size;

  3177     unsigned int expedited, size_specified;

  3178 

  3179     if (datagram->state != EC_DATAGRAM_RECEIVED

  3180         || datagram->working_counter != 1) {

  3181         fsm->coe_state = ec_fsm_error;

  3182         EC_ERR("Reception of CoE upload response failed.\n");

  3183         return;

  3184     }

  3185 

  3186     if (!(data = ec_slave_mbox_fetch(slave, datagram,

  3187 				     &mbox_prot, &rec_size))) {

  3188         fsm->coe_state = ec_fsm_error;

  3189         return;

  3190     }

  3191 

  3192     if (master->debug_level) {

  3193         EC_DBG("Upload response:\n");

  3194         ec_print_data(data, rec_size);

  3195     }

  3196 

  3197     if (mbox_prot != 0x03) { // CoE

  3198         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

  3199         fsm->coe_state = ec_fsm_error;

  3200 	return;

  3201     }

  3202 

  3203     if (rec_size < 10) {

  3204         EC_ERR("Received currupted SDO upload response!\n");

  3205         ec_print_data(data, rec_size);

  3206         fsm->coe_state = ec_fsm_error;

  3207 	return;

  3208     }

  3209 

  3210     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

  3211         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

  3212         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",

  3213                sdo->index, entry->subindex, slave->ring_position);

  3214         ec_canopen_abort_msg(EC_READ_U32(data + 6));

  3215         fsm->coe_state = ec_fsm_error;

  3216 	return;

  3217     }

  3218 

  3219     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

  3220         EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response

  3221         EC_READ_U16(data + 3) != sdo->index || // index

  3222         EC_READ_U8 (data + 5) != entry->subindex) { // subindex

  3223         EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);

  3224         EC_ERR("Invalid SDO upload response at slave %i!\n",

  3225                slave->ring_position);

  3226         ec_print_data(data, rec_size);

  3227         fsm->coe_state = ec_fsm_error;

  3228 	return;

  3229     }

  3230 

  3231     data_size = rec_size - 10;

  3232     expedited = EC_READ_U8(data + 2) & 0x02;

  3233 

  3234     if (expedited) {

  3235         EC_WARN("Received expedited response upon normal request!\n");

  3236 

  3237         size_specified = EC_READ_U8(data + 2) & 0x01;

  3238         if (size_specified) {

  3239             complete_size = 4 - ((EC_READ_U8(data + 2) & 0x0C) >> 2);

  3240         }

  3241         else {

  3242             complete_size = 4;

  3243         }

  3244     }

  3245     else {

  3246         complete_size = EC_READ_U32(data + 6);

  3247     }

  3248 

  3249     if (request->data) {

  3250         kfree(request->data);

  3251         request->data = NULL;

  3252     }

  3253     request->size = 0;

  3254 

  3255     if (complete_size) {

  3256         if (!(request->data = (uint8_t *) kmalloc(complete_size + 1, GFP_ATOMIC))) {

  3257             EC_ERR("Failed to allocate %i bytes of SDO data!\n", complete_size);

  3258             fsm->coe_state = ec_fsm_error;

  3259             return;

  3260         }

  3261         request->data[complete_size] = 0x00; // just to be sure...

  3262     }

  3263 

  3264     if (expedited) {

  3265         memcpy(request->data, data + 6, complete_size);

  3266         request->size = complete_size;

  3267     }

  3268     else {

  3269         memcpy(request->data, data + 10, data_size);

  3270         request->size = data_size;

  3271         fsm->coe_toggle = 0;

  3272 

  3273         if (data_size < complete_size) {

  3274             EC_WARN("SDO data incomplete (%i / %i).\n", data_size, complete_size);

  3275 

  3276             if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {

  3277                 fsm->coe_state = ec_fsm_error;

  3278                 return;

  3279             }

  3280 

  3281             EC_WRITE_U16(data, 0x2 << 12); // SDO request

  3282             EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle

  3283                                     | 0x3 << 5)); // upload segment request

  3284 

  3285             if (master->debug_level) {

  3286                 EC_DBG("Upload segment request:\n");

  3287                 ec_print_data(data, 3);

  3288             }

  3289 

  3290             ec_master_queue_datagram(fsm->master, datagram);

  3291             fsm->coe_state = ec_fsm_coe_up_seg_request;

  3292             return;

  3293         }

  3294     }

  3295 

  3296     fsm->coe_state = ec_fsm_end; // success

  3297 }

  3298 

  3299 /*****************************************************************************/

  3300 

  3301 /**

  3302    CoE state: UP REQUEST.

  3303 */

  3304 

  3305 void ec_fsm_coe_up_seg_request(ec_fsm_t *fsm /**< finite state machine */)

  3306 {

  3307     ec_datagram_t *datagram = &fsm->datagram;

  3308     ec_slave_t *slave = fsm->slave;

  3309 

  3310     if (datagram->state != EC_DATAGRAM_RECEIVED

  3311         || datagram->working_counter != 1) {

  3312         fsm->coe_state = ec_fsm_error;

  3313         EC_ERR("Reception of CoE upload segment request failed.\n");

  3314         return;

  3315     }

  3316 

  3317     fsm->coe_start = datagram->cycles_sent;

  3318 

  3319     ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  3320     ec_master_queue_datagram(fsm->master, datagram);

  3321     fsm->coe_state = ec_fsm_coe_up_seg_check;

  3322 }

  3323 

  3324 /*****************************************************************************/

  3325 

  3326 /**

  3327    CoE state: UP CHECK.

  3328 */

  3329 

  3330 void ec_fsm_coe_up_seg_check(ec_fsm_t *fsm /**< finite state machine */)

  3331 {

  3332     ec_datagram_t *datagram = &fsm->datagram;

  3333     ec_slave_t *slave = fsm->slave;

  3334 

  3335     if (datagram->state != EC_DATAGRAM_RECEIVED

  3336         || datagram->working_counter != 1) {

  3337         fsm->coe_state = ec_fsm_error;

  3338         EC_ERR("Reception of CoE mailbox check datagram failed.\n");

  3339         return;

  3340     }

  3341 

  3342     if (!ec_slave_mbox_check(datagram)) {

  3343         if (datagram->cycles_received

  3344             - fsm->coe_start >= (cycles_t) 100 * cpu_khz) {

  3345             fsm->coe_state = ec_fsm_error;

  3346             EC_ERR("Timeout while checking SDO upload segment on slave %i.\n",

  3347                    slave->ring_position);

  3348             return;

  3349         }

  3350 

  3351         ec_slave_mbox_prepare_check(slave, datagram); // can not fail.

  3352         ec_master_queue_datagram(fsm->master, datagram);

  3353         return;

  3354     }

  3355 

  3356     // Fetch response

  3357     ec_slave_mbox_prepare_fetch(slave, datagram); // can not fail.

  3358     ec_master_queue_datagram(fsm->master, datagram);

  3359     fsm->coe_state = ec_fsm_coe_up_seg_response;

  3360 }

  3361 

  3362 /*****************************************************************************/

  3363 

  3364 /**

  3365    CoE state: UP RESPONSE.

  3366 */

  3367 

  3368 void ec_fsm_coe_up_seg_response(ec_fsm_t *fsm /**< finite state machine */)

  3369 {

  3370     ec_datagram_t *datagram = &fsm->datagram;

  3371     ec_slave_t *slave = fsm->slave;

  3372     ec_master_t *master = slave->master;

  3373     uint8_t *data, mbox_prot;

  3374     size_t rec_size, data_size;

  3375     ec_sdo_request_t *request = fsm->coe_request;

  3376     ec_sdo_t *sdo = request->sdo;

  3377     ec_sdo_entry_t *entry = request->entry;

  3378     uint32_t seg_size;

  3379     unsigned int last_segment;

  3380 

  3381     if (datagram->state != EC_DATAGRAM_RECEIVED

  3382         || datagram->working_counter != 1) {

  3383         fsm->coe_state = ec_fsm_error;

  3384         EC_ERR("Reception of CoE upload segment response failed.\n");

  3385         return;

  3386     }

  3387 

  3388     if (!(data = ec_slave_mbox_fetch(slave, datagram,

  3389 				     &mbox_prot, &rec_size))) {

  3390         fsm->coe_state = ec_fsm_error;

  3391         return;

  3392     }

  3393 

  3394     if (master->debug_level) {

  3395         EC_DBG("Upload segment response:\n");

  3396         ec_print_data(data, rec_size);

  3397     }

  3398 

  3399     if (mbox_prot != 0x03) { // CoE

  3400         EC_WARN("Received mailbox protocol 0x%02X as response.\n", mbox_prot);

  3401         fsm->coe_state = ec_fsm_error;

  3402 	return;

  3403     }

  3404 

  3405     if (rec_size < 10) {

  3406         EC_ERR("Received currupted SDO upload segment response!\n");

  3407         ec_print_data(data, rec_size);

  3408         fsm->coe_state = ec_fsm_error;

  3409 	return;

  3410     }

  3411 

  3412     if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

  3413         EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

  3414         EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",

  3415                sdo->index, entry->subindex, slave->ring_position);

  3416         ec_canopen_abort_msg(EC_READ_U32(data + 6));

  3417         fsm->coe_state = ec_fsm_error;

  3418 	return;

  3419     }

  3420 

  3421     if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

  3422         EC_READ_U8 (data + 2) >> 5 != 0x0) { // upload segment response

  3423         EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo->index, entry->subindex);

  3424         EC_ERR("Invalid SDO upload segment response at slave %i!\n",

  3425                slave->ring_position);

  3426         ec_print_data(data, rec_size);

  3427         fsm->coe_state = ec_fsm_error;

  3428 	return;

  3429     }

  3430 

  3431     last_segment = EC_READ_U8(data + 2) & 0x01;

  3432     seg_size = (EC_READ_U8(data + 2) & 0xE) >> 1;

  3433     data_size = rec_size - 10;

  3434 

  3435     if (data_size != seg_size) {

  3436         EC_WARN("SDO segment data invalid (%i / %i)"

  3437                 " - Fragmenting not implemented.\n",

  3438                 data_size, seg_size);

  3439     }

  3440 

  3441     memcpy(request->data + request->size, data + 10, data_size);

  3442     request->size += data_size;

  3443 

  3444     if (!last_segment) {

  3445         fsm->coe_toggle = !fsm->coe_toggle;

  3446 

  3447         if (!(data = ec_slave_mbox_prepare_send(slave, datagram, 0x03, 3))) {

  3448             fsm->coe_state = ec_fsm_error;

  3449             return;

  3450         }

  3451 

  3452         EC_WRITE_U16(data, 0x2 << 12); // SDO request

  3453         EC_WRITE_U8 (data + 2, (fsm->coe_toggle << 4 // toggle

  3454                                 | 0x3 << 5)); // upload segment request

  3455 

  3456         if (master->debug_level) {

  3457             EC_DBG("Upload segment request:\n");

  3458             ec_print_data(data, 3);

  3459         }

  3460 

  3461         ec_master_queue_datagram(fsm->master, datagram);

  3462         fsm->coe_state = ec_fsm_coe_up_seg_request;