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

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

    23

 *  the industrial property and similar rights of Beckhoff Automation GmbH.

    24

 *

    25

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

    26

    27

/** \file

    28

 * Vendor specific over EtherCAT protocol handler functions.

    29

 */

    30

    31

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

    32

    33

#include <linux/module.h>

    34

    35

#include "master.h"

    36

#include "slave_config.h"

    37

#include "mailbox.h"

    38

#include "voe_handler.h"

    39

    40

/** VoE mailbox type.

    41

 */

    42

#define EC_MBOX_TYPE_VOE 0x0f

    43

    44

/** VoE header size.

    45

 */

    46

#define EC_VOE_HEADER_SIZE 6

    47

    48

/** VoE response timeout in [ms].

    49

 */

    50

#define EC_VOE_RESPONSE_TIMEOUT 500

    51

    52

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

    53

    54

void ec_voe_handler_state_write_start(ec_voe_handler_t *);

    55

void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    56

    57

void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    58

void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    59

void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    60

    61

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);

    62

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);

    63

    64

void ec_voe_handler_state_end(ec_voe_handler_t *);

    65

void ec_voe_handler_state_error(ec_voe_handler_t *);

    66

    67

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

    68

    69

/** VoE handler constructor.

    70

 *

    71

 * \return Return value of ec_datagram_prealloc().

    72

 */

    73

int ec_voe_handler_init(

    74

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

    75

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

    76

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

    77

        )

    78

{

    79

    voe->config = sc;

    80

    voe->vendor_id = 0x00000000;

    81

    voe->vendor_type = 0x0000;

    82

    voe->data_size = 0;

    83

    voe->dir = EC_DIR_INVALID;

    84

    voe->state = ec_voe_handler_state_error;

    85

    voe->request_state = EC_INT_REQUEST_INIT;

    86

    87

    ec_datagram_init(&voe->datagram);

    88

    return ec_datagram_prealloc(&voe->datagram,

    89

            size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

    90

}

    91

    92

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

    93

    94

/** VoE handler destructor.

    95

 */

    96

void ec_voe_handler_clear(

    97

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

    98

        )

    99

{

   100

    ec_datagram_clear(&voe->datagram);

   101

}

   102

   103

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

   104

   105

/** Get usable memory size.

   106

 */

   107

size_t ec_voe_handler_mem_size(

   108

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

   109

		)

   110

{

   111

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

   112

		return voe->datagram.mem_size -

   113

			(EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

   114

	else

   115

		return 0;

   116

}

   117

   118

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

   119

 * Application interface.

   120

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

   121

   122

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   123

        uint16_t vendor_type)

   124

{

   125

    voe->vendor_id = vendor_id;

   126

    voe->vendor_type = vendor_type;

   127

}

   128

   129

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

   130

   131

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   132

        uint32_t *vendor_id, uint16_t *vendor_type)

   133

{

   134

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

   135

   136

    if (vendor_id)

   137

        *vendor_id = EC_READ_U32(header);

   138

    if (vendor_type)

   139

        *vendor_type = EC_READ_U16(header + 4);

   140

}

   141

   142

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

   143

   144

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   145

{

   146

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

   147

}

   148

   149

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

   150

   151

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   152

{

   153

    return voe->data_size;

   154

}

   155

   156

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

   157

   158

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   159

{

   160

    voe->dir = EC_DIR_INPUT;

   161

    voe->state = ec_voe_handler_state_read_start;

   162

    voe->request_state = EC_INT_REQUEST_QUEUED;

   163

}

   164

   165

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

   166

   167

void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)

   168

{

   169

    voe->dir = EC_DIR_INPUT;

   170

    voe->state = ec_voe_handler_state_read_nosync_start;

   171

    voe->request_state = EC_INT_REQUEST_QUEUED;

   172

}

   173

   174

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

   175

   176

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   177

{

   178

    voe->dir = EC_DIR_OUTPUT;

   179

    voe->data_size = size;

   180

    voe->state = ec_voe_handler_state_write_start;

   181

    voe->request_state = EC_INT_REQUEST_QUEUED;

   182

}

   183

   184

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

   185

   186

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   187

{

   188

    if (voe->config->slave) {

   189

        voe->state(voe);

   190

        if (voe->request_state == EC_REQUEST_BUSY)

   191

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

   192

    } else {

   193

        voe->state = ec_voe_handler_state_error;

   194

        voe->request_state = EC_INT_REQUEST_FAILURE;

   195

    }

   196

   197

    return ec_request_state_translation_table[voe->request_state];

   198

}

   199

   200

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

   201

 * State functions.

   202

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

   203

   204

/** Start writing VoE data.

   205

 */

   206

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   207

{

   208

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

   209

    uint8_t *data;

   210

   211

    if (slave->master->debug_level) {

   212

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

   213

               voe->data_size, slave->ring_position);

   214

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

   215

    }

   216

   217

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

   218

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

   219

        voe->state = ec_voe_handler_state_error;

   220

        voe->request_state = EC_INT_REQUEST_FAILURE;

   221

        return;

   222

    }

   223

   224

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

   225

            EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);

   226

    if (IS_ERR(data)) {

   227

        voe->state = ec_voe_handler_state_error;

   228

        voe->request_state = EC_INT_REQUEST_FAILURE;

   229

        return;

   230

    }

   231

   232

    EC_WRITE_U32(data,     voe->vendor_id);

   233

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

   234

   235

    voe->retries = EC_FSM_RETRIES;

   236

    voe->jiffies_start = jiffies;

   237

    voe->state = ec_voe_handler_state_write_response;

   238

}

   239

   240

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

   241

   242

/** Wait for the mailbox response.

   243

 */

   244

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   245

{

   246

    ec_datagram_t *datagram = &voe->datagram;

   247

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

   248

   249

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

   250

        return;

   251

   252

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   253

        voe->state = ec_voe_handler_state_error;

   254

        voe->request_state = EC_INT_REQUEST_FAILURE;

   255

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

   256

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

   257

               slave->ring_position, datagram->state);

   258

        return;

   259

    }

   260

   261

    if (datagram->working_counter != 1) {

   262

        if (!datagram->working_counter) {

   263

            unsigned long diff_ms =

   264

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

   265

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   266

                if (slave->master->debug_level) {

   267

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

   268

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

   269

                            slave->ring_position, (u32) diff_ms);

   270

                }

   271

                // no response; send request datagram again

   272

                return;

   273

            }

   274

        }

   275

        voe->state = ec_voe_handler_state_error;

   276

        voe->request_state = EC_INT_REQUEST_FAILURE;

   277

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

   278

                slave->ring_position);

   279

        ec_datagram_print_wc_error(datagram);

   280

        return;

   281

    }

   282

   283

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

   284

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

   285

   286

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   287

    voe->state = ec_voe_handler_state_end;

   288

}

   289

   290

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

   291

   292

/** Start reading VoE data.

   293

 */

   294

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   295

{

   296

    ec_datagram_t *datagram = &voe->datagram;

   297

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

   298

   299

    if (slave->master->debug_level)

   300

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

   301

   302

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

   303

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

   304

        voe->state = ec_voe_handler_state_error;

   305

        voe->request_state = EC_INT_REQUEST_FAILURE;

   306

        return;

   307

    }

   308

   309

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

   310

   311

    voe->jiffies_start = jiffies;

   312

    voe->retries = EC_FSM_RETRIES;

   313

    voe->state = ec_voe_handler_state_read_check;

   314

}

   315

   316

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

   317

   318

/** Check for new data in the mailbox.

   319

 */

   320

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   321

{

   322

    ec_datagram_t *datagram = &voe->datagram;

   323

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

   324

   325

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

   326

        return;

   327

   328

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   329

        voe->state = ec_voe_handler_state_error;

   330

        voe->request_state = EC_INT_REQUEST_FAILURE;

   331

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

   332

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

   333

               slave->ring_position, datagram->state);

   334

        return;

   335

    }

   336

   337

    if (datagram->working_counter != 1) {

   338

        voe->state = ec_voe_handler_state_error;

   339

        voe->request_state = EC_INT_REQUEST_FAILURE;

   340

        EC_ERR("Reception of VoE mailbox check"

   341

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

   342

        ec_datagram_print_wc_error(datagram);

   343

        return;

   344

    }

   345

   346

    if (!ec_slave_mbox_check(datagram)) {

   347

        unsigned long diff_ms =

   348

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

   349

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   350

            voe->state = ec_voe_handler_state_error;

   351

            voe->request_state = EC_INT_REQUEST_FAILURE;

   352

            EC_ERR("Timeout while waiting for VoE data on "

   353

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

   354

            return;

   355

        }

   356

   357

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

   358

        voe->retries = EC_FSM_RETRIES;

   359

        return;

   360

    }

   361

   362

    // Fetch response

   363

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

   364

    voe->retries = EC_FSM_RETRIES;

   365

    voe->state = ec_voe_handler_state_read_response;

   366

}

   367

   368

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

   369

   370

/** Read the pending mailbox data.

   371

 */

   372

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   373

{

   374

    ec_datagram_t *datagram = &voe->datagram;

   375

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

   376

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

   377

    uint8_t *data, mbox_prot;

   378

    size_t rec_size;

   379

   380

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

   381

        return;

   382

   383

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   384

        voe->state = ec_voe_handler_state_error;

   385

        voe->request_state = EC_INT_REQUEST_FAILURE;

   386

        EC_ERR("Failed to receive VoE read datagram for"

   387

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

   388

               slave->ring_position, datagram->state);

   389

        return;

   390

    }

   391

   392

    if (datagram->working_counter != 1) {

   393

        voe->state = ec_voe_handler_state_error;

   394

        voe->request_state = EC_INT_REQUEST_FAILURE;

   395

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

   396

                slave->ring_position);

   397

        ec_datagram_print_wc_error(datagram);

   398

        return;

   399

    }

   400

   401

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

   402

    if (IS_ERR(data)) {

   403

        voe->state = ec_voe_handler_state_error;

   404

        voe->request_state = EC_INT_REQUEST_FAILURE;

   405

        return;

   406

    }

   407

   408

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   409

        voe->state = ec_voe_handler_state_error;

   410

        voe->request_state = EC_INT_REQUEST_FAILURE;

   411

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

   412

        ec_print_data(data, rec_size);

   413

        return;

   414

    }

   415

   416

    if (rec_size < EC_VOE_HEADER_SIZE) {

   417

        voe->state = ec_voe_handler_state_error;

   418

        voe->request_state = EC_INT_REQUEST_FAILURE;

   419

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

   420

        return;

   421

    }

   422

   423

    if (master->debug_level) {

   424

        EC_DBG("VoE data:\n");

   425

        ec_print_data(data, rec_size);

   426

    }

   427

   428

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   429

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   430

    voe->state = ec_voe_handler_state_end; // success

   431

}

   432

   433

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

   434

   435

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

   436

 */

   437

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)

   438

{

   439

    ec_datagram_t *datagram = &voe->datagram;

   440

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

   441

   442

    if (slave->master->debug_level)

   443

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

   444

   445

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

   446

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

   447

        voe->state = ec_voe_handler_state_error;

   448

        voe->request_state = EC_INT_REQUEST_FAILURE;

   449

        return;

   450

    }

   451

   452

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

   453

   454

    voe->jiffies_start = jiffies;

   455

    voe->retries = EC_FSM_RETRIES;

   456

    voe->state = ec_voe_handler_state_read_nosync_response;

   457

}

   458

   459

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

   460

   461

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

   462

 *  might lead to an empty reponse from the client.

   463

 */

   464

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)

   465

{

   466

    ec_datagram_t *datagram = &voe->datagram;

   467

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

   468

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

   469

    uint8_t *data, mbox_prot;

   470

    size_t rec_size;

   471

   472

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

   473

        return;

   474

   475

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   476

        voe->state = ec_voe_handler_state_error;

   477

        voe->request_state = EC_INT_REQUEST_FAILURE;

   478

        EC_ERR("Failed to receive VoE read datagram for"

   479

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

   480

               slave->ring_position, datagram->state);

   481

        return;

   482

    }

   483

   484

    if (datagram->working_counter == 0) {

   485

        voe->state = ec_voe_handler_state_error;

   486

        voe->request_state = EC_INT_REQUEST_FAILURE;

   487

        if (master->debug_level)

   488

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

   489

        return;

   490

    }

   491

   492

    if (datagram->working_counter != 1) {

   493

        voe->state = ec_voe_handler_state_error;

   494

        voe->request_state = EC_INT_REQUEST_FAILURE;

   495

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

   496

                slave->ring_position);

   497

        ec_datagram_print_wc_error(datagram);

   498

        return;

   499

    }

   500

   501

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

   502

				     &mbox_prot, &rec_size))) {

   503

        voe->state = ec_voe_handler_state_error;

   504

        voe->request_state = EC_INT_REQUEST_FAILURE;

   505

        return;

   506

    }

   507

   508

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   509

        voe->state = ec_voe_handler_state_error;

   510

        voe->request_state = EC_INT_REQUEST_FAILURE;

   511

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

   512

        ec_print_data(data, rec_size);

   513

        return;