1

/******************************************************************************

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2008  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

 *****************************************************************************/

    29

    30

/**

    31

   \file

    32

   EtherCAT SoE state machines.

    33

*/

    34

    35

/*****************************************************************************/

    36

    37

#include "globals.h"

    38

#include "master.h"

    39

#include "mailbox.h"

    40

#include "fsm_soe.h"

    41

    42

/*****************************************************************************/

    43

    44

/** Mailbox type for SoE.

    45

 */

    46

#define EC_MBOX_TYPE_SOE 0x05

    47

    48

#define EC_SOE_OPCODE_READ_REQUEST  0x01

    49

#define EC_SOE_OPCODE_READ_RESPONSE 0x02

    50

    51

#define EC_SOE_READ_REQUEST_SIZE 0x04

    52

#define EC_SOE_READ_RESPONSE_SIZE 0x04

    53

    54

#define EC_SOE_RESPONSE_TIMEOUT 1000

    55

    56

/*****************************************************************************/

    57

    58

void ec_fsm_soe_read_start(ec_fsm_soe_t *);

    59

void ec_fsm_soe_read_request(ec_fsm_soe_t *);

    60

void ec_fsm_soe_read_check(ec_fsm_soe_t *);

    61

void ec_fsm_soe_read_response(ec_fsm_soe_t *);

    62

    63

void ec_fsm_soe_end(ec_fsm_soe_t *);

    64

void ec_fsm_soe_error(ec_fsm_soe_t *);

    65

    66

/*****************************************************************************/

    67

    68

/** Constructor.

    69

 */

    70

void ec_fsm_soe_init(

    71

		ec_fsm_soe_t *fsm, /**< finite state machine */

    72

		ec_datagram_t *datagram /**< datagram */

    73

		)

    74

{

    75

    fsm->state = NULL;

    76

    fsm->datagram = datagram;

    77

}

    78

    79

/*****************************************************************************/

    80

    81

/** Destructor.

    82

 */

    83

void ec_fsm_soe_clear(

    84

		ec_fsm_soe_t *fsm /**< finite state machine */

    85

		)

    86

{

    87

}

    88

    89

/*****************************************************************************/

    90

    91

/** Starts to transfer an IDN to/from a slave.

    92

 */

    93

void ec_fsm_soe_transfer(

    94

        ec_fsm_soe_t *fsm, /**< State machine. */

    95

        ec_slave_t *slave, /**< EtherCAT slave. */

    96

        ec_soe_request_t *request /**< SoE request. */

    97

        )

    98

{

    99

    fsm->slave = slave;

   100

    fsm->request = request;

   101

    if (request->dir == EC_DIR_OUTPUT) {

   102

        //fsm->state = ec_fsm_soe_write_start;

   103

	} else {

   104

        fsm->state = ec_fsm_soe_read_start;

   105

	}

   106

}

   107

   108

/*****************************************************************************/

   109

   110

/**

   111

   Executes the current state of the state machine.

   112

   \return false, if state machine has terminated

   113

*/

   114

   115

int ec_fsm_soe_exec(ec_fsm_soe_t *fsm /**< finite state machine */)

   116

{

   117

    fsm->state(fsm);

   118

   119

    return fsm->state != ec_fsm_soe_end && fsm->state != ec_fsm_soe_error;

   120

}

   121

   122

/*****************************************************************************/

   123

   124

/**

   125

   Returns, if the state machine terminated with success.

   126

   \return non-zero if successful.

   127

*/

   128

   129

int ec_fsm_soe_success(ec_fsm_soe_t *fsm /**< Finite state machine */)

   130

{

   131

    return fsm->state == ec_fsm_soe_end;

   132

}

   133

   134

/******************************************************************************

   135

 * SoE read state machine

   136

 *****************************************************************************/

   137

   138

/** SoE state: READ START.

   139

 */

   140

void ec_fsm_soe_read_start(ec_fsm_soe_t *fsm /**< finite state machine */)

   141

{

   142

    ec_datagram_t *datagram = fsm->datagram;

   143

    ec_slave_t *slave = fsm->slave;

   144

    ec_master_t *master = slave->master;

   145

    ec_soe_request_t *request = fsm->request;

   146

    uint8_t *data;

   147

   148

    if (master->debug_level)

   149

        EC_DBG("Reading IDN 0x%04X from slave %u.\n",

   150

               request->idn, slave->ring_position);

   151

   152

    if (!(slave->sii.mailbox_protocols & EC_MBOX_SOE)) {

   153

        EC_ERR("Slave %u does not support SoE!\n", slave->ring_position);

   154

        fsm->state = ec_fsm_soe_error;

   155

        return;

   156

    }

   157

   158

    data = ec_slave_mbox_prepare_send(slave, datagram, EC_MBOX_TYPE_SOE,

   159

			EC_SOE_READ_REQUEST_SIZE);

   160

    if (IS_ERR(data)) {

   161

        fsm->state = ec_fsm_soe_error;

   162

        return;

   163

    }

   164

   165

    EC_WRITE_U8(data, EC_SOE_OPCODE_READ_REQUEST);

   166

    EC_WRITE_U8(data + 1, 1 << 6); // request value

   167

    EC_WRITE_U16(data + 2, request->idn);

   168

   169

    if (master->debug_level) {

   170

        EC_DBG("SCC read request:\n");

   171

        ec_print_data(data, EC_SOE_READ_REQUEST_SIZE);

   172

    }

   173

   174

    fsm->request->jiffies_sent = jiffies;

   175

    fsm->retries = EC_FSM_RETRIES;

   176

    fsm->state = ec_fsm_soe_read_request;

   177

}

   178

   179

/*****************************************************************************/

   180

   181

/** SoE state: READ REQUEST.

   182

 */

   183

void ec_fsm_soe_read_request(ec_fsm_soe_t *fsm /**< finite state machine */)

   184

{

   185

    ec_datagram_t *datagram = fsm->datagram;

   186

    ec_slave_t *slave = fsm->slave;

   187

    unsigned long diff_ms;

   188

   189

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   190

        return; // FIXME: check for response first?

   191

   192

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   193

        fsm->state = ec_fsm_soe_error;

   194

        EC_ERR("Failed to receive SoE read request for slave %u: ",

   195

               slave->ring_position);

   196

        ec_datagram_print_state(datagram);

   197

        return;

   198

    }

   199

   200

    diff_ms = (jiffies - fsm->request->jiffies_sent) * 1000 / HZ;

   201

   202

    if (datagram->working_counter != 1) {

   203

        if (!datagram->working_counter) {

   204

            if (diff_ms < EC_SOE_RESPONSE_TIMEOUT) {

   205

                // no response; send request datagram again

   206

                return;

   207

            }

   208

        }

   209

        fsm->state = ec_fsm_soe_error;

   210

        EC_ERR("Reception of SoE read request for IDN 0x%04x failed"

   211

                " after %u ms on slave %u: ",

   212

                fsm->request->idn, (u32) diff_ms,

   213

                fsm->slave->ring_position);

   214

        ec_datagram_print_wc_error(datagram);

   215

        return;

   216

    }

   217

   218

    fsm->jiffies_start = datagram->jiffies_sent;

   219

   220

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

   221

    fsm->retries = EC_FSM_RETRIES;

   222

    fsm->state = ec_fsm_soe_read_check;

   223

}

   224

   225

/*****************************************************************************/

   226

   227

/** CoE state: READ CHECK.

   228

 */

   229

void ec_fsm_soe_read_check(ec_fsm_soe_t *fsm /**< finite state machine */)

   230

{

   231

    ec_datagram_t *datagram = fsm->datagram;

   232

    ec_slave_t *slave = fsm->slave;

   233

   234

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   235

        return;

   236

   237

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   238

        fsm->state = ec_fsm_soe_error;

   239

        EC_ERR("Failed to receive SoE mailbox check datagram from slave %u: ",

   240

               slave->ring_position);

   241

        ec_datagram_print_state(datagram);

   242

        return;

   243

    }

   244

   245

    if (datagram->working_counter != 1) {

   246

        fsm->state = ec_fsm_soe_error;

   247

        EC_ERR("Reception of SoE mailbox check datagram failed on slave %u: ",

   248

				slave->ring_position);

   249

        ec_datagram_print_wc_error(datagram);

   250

        return;

   251

    }

   252

   253

    if (!ec_slave_mbox_check(datagram)) {

   254

        unsigned long diff_ms =

   255

            (datagram->jiffies_received - fsm->jiffies_start) * 1000 / HZ;

   256

        if (diff_ms >= EC_SOE_RESPONSE_TIMEOUT) {

   257

            fsm->state = ec_fsm_soe_error;

   258

            EC_ERR("Timeout after %u ms while waiting for IDN 0x%04x"

   259

                    " read response on slave %u.\n", (u32) diff_ms,

   260

                    fsm->request->idn, slave->ring_position);

   261

            return;

   262

        }

   263

   264

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

   265

        fsm->retries = EC_FSM_RETRIES;

   266

        return;

   267

    }

   268

   269

    // Fetch response

   270

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

   271

    fsm->retries = EC_FSM_RETRIES;

   272

    fsm->state = ec_fsm_soe_read_response;

   273

}

   274

   275

/*****************************************************************************/

   276

   277

/** SoE state: READ RESPONSE.

   278

 */

   279

void ec_fsm_soe_read_response(ec_fsm_soe_t *fsm /**< finite state machine */)

   280

{

   281

    ec_datagram_t *datagram = fsm->datagram;

   282

    ec_slave_t *slave = fsm->slave;

   283

    ec_master_t *master = slave->master;

   284

    uint8_t *data, mbox_prot, opcode, error_flag, value_included;

   285

    size_t rec_size, data_size;

   286

    ec_soe_request_t *req = fsm->request;

   287

   288

    if (datagram->state == EC_DATAGRAM_TIMED_OUT && fsm->retries--)

   289

        return; // FIXME: request again?

   290

   291

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   292

        fsm->state = ec_fsm_soe_error;

   293

        EC_ERR("Failed to receive SoE read response datagram for"

   294

               " slave %u: ", slave->ring_position);

   295

        ec_datagram_print_state(datagram);

   296

        return;

   297

    }

   298

   299

    if (datagram->working_counter != 1) {

   300

        fsm->state = ec_fsm_soe_error;

   301

        EC_ERR("Reception of SoE read response failed on slave %u: ",

   302

                slave->ring_position);

   303

        ec_datagram_print_wc_error(datagram);

   304

        return;

   305

    }

   306

   307

    data = ec_slave_mbox_fetch(slave, datagram, &mbox_prot, &rec_size);

   308

    if (IS_ERR(data)) {

   309

        fsm->state = ec_fsm_soe_error;

   310

        return;

   311

    }

   312

   313

    if (master->debug_level) {

   314

        EC_DBG("SCC read response:\n");

   315

        ec_print_data(data, rec_size);

   316

    }

   317

   318

    if (mbox_prot != EC_MBOX_TYPE_SOE) {

   319

        fsm->state = ec_fsm_soe_error;

   320

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

   321

        return;

   322

    }

   323

   324

    if (rec_size < EC_SOE_READ_RESPONSE_SIZE) {

   325

        fsm->state = ec_fsm_soe_error;

   326

        EC_ERR("Received currupted SoE read response (%zu bytes)!\n",

   327

				rec_size);

   328

        ec_print_data(data, rec_size);

   329

        return;

   330

    }

   331

   332

	opcode = EC_READ_U8(data) & 0x3;

   333

    if (opcode != EC_SOE_OPCODE_READ_RESPONSE) {

   334

        EC_ERR("Received no read response (opcode %x).\n", opcode);

   335

        ec_print_data(data, rec_size);

   336

        fsm->state = ec_fsm_soe_error;

   337

        return;

   338

    }

   339

   340

	error_flag = (EC_READ_U8(data) >> 4) & 1;

   341

	if (error_flag) {

   342

		req->error_code = EC_READ_U16(data + rec_size - 2);

   343

		EC_ERR("Received error response: 0x%04x.\n",

   344

				req->error_code);

   345

        fsm->state = ec_fsm_soe_error;

   346

        return;

   347

	} else {

   348

		req->error_code = 0x0000;

   349

	}

   350

   351

	value_included = (EC_READ_U8(data + 1) >> 6) & 1;

   352

	if (!value_included) {

   353

		EC_ERR("No value included!\n");

   354

		fsm->state = ec_fsm_soe_error;

   355

		return;

   356

	}

   357

   358

	data_size = rec_size - EC_SOE_READ_RESPONSE_SIZE;

   359

	if (ec_soe_request_copy_data(req,

   360

				data + EC_SOE_READ_RESPONSE_SIZE, data_size)) {

   361

		fsm->state = ec_fsm_soe_error;

   362

		return;

   363

	}

   364

   365

	if (master->debug_level) {

   366

		EC_DBG("IDN data:\n");

   367

		ec_print_data(req->data, req->data_size);

   368

	}

   369

   370

    fsm->state = ec_fsm_soe_end; // success

   371

}

   372

   373

/*****************************************************************************/

   374

   375

/** State: ERROR.

   376

 */

   377

void ec_fsm_soe_error(ec_fsm_soe_t *fsm /**< finite state machine */)

   378

{

   379

}

   380

   381

/*****************************************************************************/

   382

   383

/** State: END.

   384

 */

   385

void ec_fsm_soe_end(ec_fsm_soe_t *fsm /**< finite state machine */)

   386

{

   387

}

   388

   389

/*****************************************************************************/