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

/** \file

    31

 * Vendor specific over EtherCAT protocol handler functions.

    32

 */

    33

    34

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

    35

    36

#include <linux/module.h>

    37

    38

#include "master.h"

    39

#include "slave_config.h"

    40

#include "mailbox.h"

    41

#include "voe_handler.h"

    42

    43

/** VoE mailbox type.

    44

 */

    45

#define EC_MBOX_TYPE_VOE 0x0f

    46

    47

/** VoE header size.

    48

 */

    49

#define EC_VOE_HEADER_SIZE 6

    50

    51

/** VoE response timeout in [ms].

    52

 */

    53

#define EC_VOE_RESPONSE_TIMEOUT 500

    54

    55

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

    56

    57

void ec_voe_handler_state_write_start(ec_voe_handler_t *);

    58

void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    59

    60

void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    61

void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    62

void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    63

    64

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);

    65

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);

    66

    67

void ec_voe_handler_state_end(ec_voe_handler_t *);

    68

void ec_voe_handler_state_error(ec_voe_handler_t *);

    69

    70

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

    71

    72

/** VoE handler constructor.

    73

 *

    74

 * \return Return value of ec_datagram_prealloc().

    75

 */

    76

int ec_voe_handler_init(

    77

        ec_voe_handler_t *voe, /**< VoE handler. */

    78

        ec_slave_config_t *sc, /**< Parent slave configuration. */

    79

        size_t size /**< Size of memory to reserve. */

    80

        )

    81

{

    82

    voe->config = sc;

    83

    voe->vendor_id = 0x00000000;

    84

    voe->vendor_type = 0x0000;

    85

    voe->data_size = 0;

    86

    voe->dir = EC_DIR_INVALID;

    87

    voe->state = ec_voe_handler_state_error;

    88

    voe->request_state = EC_INT_REQUEST_INIT;

    89

    90

    ec_datagram_init(&voe->datagram);

    91

    return ec_datagram_prealloc(&voe->datagram,

    92

            size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

    93

}

    94

    95

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

    96

    97

/** VoE handler destructor.

    98

 */

    99

void ec_voe_handler_clear(

   100

        ec_voe_handler_t *voe /**< VoE handler. */

   101

        )

   102

{

   103

    ec_datagram_clear(&voe->datagram);

   104

}

   105

   106

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

   107

   108

/** Get usable memory size.

   109

 */

   110

size_t ec_voe_handler_mem_size(

   111

        const ec_voe_handler_t *voe /**< VoE handler. */

   112

        )

   113

{

   114

    if (voe->datagram.mem_size >= EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE)

   115

        return voe->datagram.mem_size -

   116

            (EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

   117

    else

   118

        return 0;

   119

}

   120

   121

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

   122

 * Application interface.

   123

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

   124

   125

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   126

        uint16_t vendor_type)

   127

{

   128

    voe->vendor_id = vendor_id;

   129

    voe->vendor_type = vendor_type;

   130

}

   131

   132

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

   133

   134

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   135

        uint32_t *vendor_id, uint16_t *vendor_type)

   136

{

   137

    uint8_t *header = voe->datagram.data + EC_MBOX_HEADER_SIZE;

   138

   139

    if (vendor_id)

   140

        *vendor_id = EC_READ_U32(header);

   141

    if (vendor_type)

   142

        *vendor_type = EC_READ_U16(header + 4);

   143

}

   144

   145

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

   146

   147

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   148

{

   149

    return voe->datagram.data + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE;

   150

}

   151

   152

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

   153

   154

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   155

{

   156

    return voe->data_size;

   157

}

   158

   159

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

   160

   161

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   162

{

   163

    voe->dir = EC_DIR_INPUT;

   164

    voe->state = ec_voe_handler_state_read_start;

   165

    voe->request_state = EC_INT_REQUEST_BUSY;

   166

}

   167

   168

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

   169

   170

void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)

   171

{

   172

    voe->dir = EC_DIR_INPUT;

   173

    voe->state = ec_voe_handler_state_read_nosync_start;

   174

    voe->request_state = EC_INT_REQUEST_BUSY;

   175

}

   176

   177

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

   178

   179

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   180

{

   181

    voe->dir = EC_DIR_OUTPUT;

   182

    voe->data_size = size;

   183

    voe->state = ec_voe_handler_state_write_start;

   184

    voe->request_state = EC_INT_REQUEST_BUSY;

   185

}

   186

   187

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

   188

   189

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   190

{

   191

    if (voe->config->slave) { // FIXME locking?

   192

        voe->state(voe);

   193

        if (voe->request_state == EC_INT_REQUEST_BUSY)

   194

            ec_master_queue_datagram(voe->config->master, &voe->datagram);

   195

    } else {

   196

        voe->state = ec_voe_handler_state_error;

   197

        voe->request_state = EC_INT_REQUEST_FAILURE;

   198

    }

   199

   200

    return ec_request_state_translation_table[voe->request_state];

   201

}

   202

   203

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

   204

 * State functions.

   205

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

   206

   207

/** Start writing VoE data.

   208

 */

   209

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   210

{

   211

    ec_slave_t *slave = voe->config->slave;

   212

    uint8_t *data;

   213

   214

    if (slave->master->debug_level) {

   215

        EC_DBG("Writing %u bytes of VoE data to slave %u.\n",

   216

               voe->data_size, slave->ring_position);

   217

        ec_print_data(ecrt_voe_handler_data(voe), voe->data_size);

   218

    }

   219

   220

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

   221

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

   222

        voe->state = ec_voe_handler_state_error;

   223

        voe->request_state = EC_INT_REQUEST_FAILURE;

   224

        return;

   225

    }

   226

   227

    data = ec_slave_mbox_prepare_send(slave, &voe->datagram,

   228

            EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);

   229

    if (IS_ERR(data)) {

   230

        voe->state = ec_voe_handler_state_error;

   231

        voe->request_state = EC_INT_REQUEST_FAILURE;

   232

        return;

   233

    }

   234

   235

    EC_WRITE_U32(data,     voe->vendor_id);

   236

    EC_WRITE_U16(data + 4, voe->vendor_type);

   237

   238

    voe->retries = EC_FSM_RETRIES;

   239

    voe->jiffies_start = jiffies;

   240

    voe->state = ec_voe_handler_state_write_response;

   241

}

   242

   243

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

   244

   245

/** Wait for the mailbox response.

   246

 */

   247

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   248

{

   249

    ec_datagram_t *datagram = &voe->datagram;

   250

    ec_slave_t *slave = voe->config->slave;

   251

   252

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

   253

        return;

   254

   255

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   256

        voe->state = ec_voe_handler_state_error;

   257

        voe->request_state = EC_INT_REQUEST_FAILURE;

   258

        EC_ERR("Failed to receive VoE write request datagram for"

   259

               " slave %u (datagram state %u).\n",

   260

               slave->ring_position, datagram->state);

   261

        return;

   262

    }

   263

   264

    if (datagram->working_counter != 1) {

   265

        if (!datagram->working_counter) {

   266

            unsigned long diff_ms =

   267

                (jiffies - voe->jiffies_start) * 1000 / HZ;

   268

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   269

                if (slave->master->debug_level) {

   270

                    EC_DBG("Slave %u did not respond to VoE write request. "

   271

                            "Retrying after %u ms...\n",

   272

                            slave->ring_position, (u32) diff_ms);

   273

                }

   274

                // no response; send request datagram again

   275

                return;

   276

            }

   277

        }

   278

        voe->state = ec_voe_handler_state_error;

   279

        voe->request_state = EC_INT_REQUEST_FAILURE;

   280

        EC_ERR("Reception of VoE write request failed on slave %u: ",

   281

                slave->ring_position);

   282

        ec_datagram_print_wc_error(datagram);

   283

        return;

   284

    }

   285

   286

    if (voe->config->master->debug_level)

   287

        EC_DBG("VoE write request successful.\n");

   288

   289

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   290

    voe->state = ec_voe_handler_state_end;

   291

}

   292

   293

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

   294

   295

/** Start reading VoE data.

   296

 */

   297

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   298

{

   299

    ec_datagram_t *datagram = &voe->datagram;

   300

    ec_slave_t *slave = voe->config->slave;

   301

   302

    if (slave->master->debug_level)

   303

        EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position);

   304

   305

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

   306

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

   307

        voe->state = ec_voe_handler_state_error;

   308

        voe->request_state = EC_INT_REQUEST_FAILURE;

   309

        return;

   310

    }

   311

   312

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

   313

   314

    voe->jiffies_start = jiffies;

   315

    voe->retries = EC_FSM_RETRIES;

   316

    voe->state = ec_voe_handler_state_read_check;

   317

}

   318

   319

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

   320

   321

/** Check for new data in the mailbox.

   322

 */

   323

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   324

{

   325

    ec_datagram_t *datagram = &voe->datagram;

   326

    ec_slave_t *slave = voe->config->slave;

   327

   328

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

   329

        return;

   330

   331

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   332

        voe->state = ec_voe_handler_state_error;

   333

        voe->request_state = EC_INT_REQUEST_FAILURE;

   334

        EC_ERR("Failed to receive VoE mailbox check datagram from slave %u"

   335

                " (datagram state %u).\n",

   336

               slave->ring_position, datagram->state);

   337

        return;

   338

    }

   339

   340

    if (datagram->working_counter != 1) {

   341

        voe->state = ec_voe_handler_state_error;

   342

        voe->request_state = EC_INT_REQUEST_FAILURE;

   343

        EC_ERR("Reception of VoE mailbox check"

   344

                " datagram failed on slave %u: ", slave->ring_position);

   345

        ec_datagram_print_wc_error(datagram);

   346

        return;

   347

    }

   348

   349

    if (!ec_slave_mbox_check(datagram)) {

   350

        unsigned long diff_ms =

   351

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

   352

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   353

            voe->state = ec_voe_handler_state_error;

   354

            voe->request_state = EC_INT_REQUEST_FAILURE;

   355

            EC_ERR("Timeout while waiting for VoE data on "

   356

                    "slave %u.\n", slave->ring_position);

   357

            return;

   358

        }

   359

   360

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

   361

        voe->retries = EC_FSM_RETRIES;

   362

        return;

   363

    }

   364

   365

    // Fetch response

   366

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

   367

    voe->retries = EC_FSM_RETRIES;

   368

    voe->state = ec_voe_handler_state_read_response;

   369

}

   370

   371

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

   372

   373

/** Read the pending mailbox data.

   374

 */

   375

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   376

{

   377

    ec_datagram_t *datagram = &voe->datagram;

   378

    ec_slave_t *slave = voe->config->slave;

   379

    ec_master_t *master = voe->config->master;

   380

    uint8_t *data, mbox_prot;

   381

    size_t rec_size;

   382

   383

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

   384

        return;

   385

   386

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   387

        voe->state = ec_voe_handler_state_error;

   388

        voe->request_state = EC_INT_REQUEST_FAILURE;

   389

        EC_ERR("Failed to receive VoE read datagram for"

   390

               " slave %u (datagram state %u).\n",

   391

               slave->ring_position, datagram->state);

   392

        return;

   393

    }

   394

   395

    if (datagram->working_counter != 1) {

   396

        voe->state = ec_voe_handler_state_error;

   397

        voe->request_state = EC_INT_REQUEST_FAILURE;

   398

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

   399

                slave->ring_position);

   400

        ec_datagram_print_wc_error(datagram);

   401

        return;

   402

    }

   403

   404

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

   405

    if (IS_ERR(data)) {

   406

        voe->state = ec_voe_handler_state_error;

   407

        voe->request_state = EC_INT_REQUEST_FAILURE;

   408

        return;

   409

    }

   410

   411

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   412

        voe->state = ec_voe_handler_state_error;

   413

        voe->request_state = EC_INT_REQUEST_FAILURE;

   414

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

   415

        ec_print_data(data, rec_size);

   416

        return;

   417

    }

   418

   419

    if (rec_size < EC_VOE_HEADER_SIZE) {

   420

        voe->state = ec_voe_handler_state_error;

   421

        voe->request_state = EC_INT_REQUEST_FAILURE;

   422

        EC_ERR("Received VoE header is incomplete (%u bytes)!\n", rec_size);

   423

        return;

   424

    }

   425

   426

    if (master->debug_level) {

   427

        EC_DBG("VoE data:\n");

   428

        ec_print_data(data, rec_size);

   429

    }

   430

   431

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   432

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   433

    voe->state = ec_voe_handler_state_end; // success

   434

}

   435

   436

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

   437

   438

/** Start reading VoE data without sending a sync message before.

   439

 */

   440

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)

   441

{

   442

    ec_datagram_t *datagram = &voe->datagram;

   443

    ec_slave_t *slave = voe->config->slave;

   444

   445

    if (slave->master->debug_level)

   446

        EC_DBG("Reading VoE data to slave %u.\n", slave->ring_position);

   447

   448

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

   449

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

   450

        voe->state = ec_voe_handler_state_error;

   451

        voe->request_state = EC_INT_REQUEST_FAILURE;

   452

        return;

   453

    }

   454

   455

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

   456

   457

    voe->jiffies_start = jiffies;

   458

    voe->retries = EC_FSM_RETRIES;

   459

    voe->state = ec_voe_handler_state_read_nosync_response;

   460

}

   461

   462

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

   463

   464

/** Read the pending mailbox data without sending a sync message before. This

   465

 *  might lead to an empty reponse from the client.

   466

 */

   467

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)

   468

{

   469

    ec_datagram_t *datagram = &voe->datagram;

   470

    ec_slave_t *slave = voe->config->slave;

   471

    ec_master_t *master = voe->config->master;

   472

    uint8_t *data, mbox_prot;

   473

    size_t rec_size;

   474

   475

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

   476

        return;

   477

   478

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   479

        voe->state = ec_voe_handler_state_error;

   480

        voe->request_state = EC_INT_REQUEST_FAILURE;

   481

        EC_ERR("Failed to receive VoE read datagram for"

   482

               " slave %u (datagram state %u).\n",

   483

               slave->ring_position, datagram->state);

   484

        return;

   485

    }

   486

   487

    if (datagram->working_counter == 0) {

   488

        voe->state = ec_voe_handler_state_error;

   489

        voe->request_state = EC_INT_REQUEST_FAILURE;

   490

        if (master->debug_level)

   491

            EC_DBG("Slave %u did not send VoE data.", slave->ring_position);

   492

        return;

   493

    }

   494

   495

    if (datagram->working_counter != 1) {

   496

        voe->state = ec_voe_handler_state_error;

   497

        voe->request_state = EC_INT_REQUEST_FAILURE;

   498

        EC_WARN("Reception of VoE read response failed on slave %u: ",

   499

                slave->ring_position);

   500

        ec_datagram_print_wc_error(datagram);

   501

        return;

   502

    }

   503

   504

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

   505

                    &mbox_prot, &rec_size))) {

   506

        voe->state = ec_voe_handler_state_error;

   507

        voe->request_state = EC_INT_REQUEST_FAILURE;

   508

        return;

   509

    }

   510

   511

    if (mbox_prot != EC_MBOX_TYPE_VOE) {