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 header size.

    44

 */

    45

#define EC_VOE_HEADER_SIZE 6

    46

    47

/** VoE response timeout in [ms].

    48

 */

    49

#define EC_VOE_RESPONSE_TIMEOUT 500

    50

    51

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

    52

    53

void ec_voe_handler_state_write_start(ec_voe_handler_t *);

    54

void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    55

    56

void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    57

void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    58

void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    59

    60

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *);

    61

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *);

    62

    63

void ec_voe_handler_state_end(ec_voe_handler_t *);

    64

void ec_voe_handler_state_error(ec_voe_handler_t *);

    65

    66

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

    67

    68

/** VoE handler constructor.

    69

 *

    70

 * \return Return value of ec_datagram_prealloc().

    71

 */

    72

int ec_voe_handler_init(

    73

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

    74

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

    75

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

    76

        )

    77

{

    78

    voe->config = sc;

    79

    voe->vendor_id = 0x00000000;

    80

    voe->vendor_type = 0x0000;

    81

    voe->data_size = 0;

    82

    voe->dir = EC_DIR_INVALID;

    83

    voe->state = ec_voe_handler_state_error;

    84

    voe->request_state = EC_INT_REQUEST_INIT;

    85

    86

    ec_datagram_init(&voe->datagram);

    87

    return ec_datagram_prealloc(&voe->datagram,

    88

            size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

    89

}

    90

    91

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

    92

    93

/** VoE handler destructor.

    94

 */

    95

void ec_voe_handler_clear(

    96

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

    97

        )

    98

{

    99

    ec_datagram_clear(&voe->datagram);

   100

}

   101

   102

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

   103

   104

/** Get usable memory size.

   105

 *

   106

 * \return Memory size.

   107

 */

   108

size_t ec_voe_handler_mem_size(

   109

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

   110

        )

   111

{

   112

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

   113

        return voe->datagram.mem_size -

   114

            (EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

   115

    else

   116

        return 0;

   117

}

   118

   119

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

   120

 * Application interface.

   121

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

   122

   123

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   124

        uint16_t vendor_type)

   125

{

   126

    voe->vendor_id = vendor_id;

   127

    voe->vendor_type = vendor_type;

   128

}

   129

   130

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

   131

   132

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   133

        uint32_t *vendor_id, uint16_t *vendor_type)

   134

{

   135

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

   136

   137

    if (vendor_id)

   138

        *vendor_id = EC_READ_U32(header);

   139

    if (vendor_type)

   140

        *vendor_type = EC_READ_U16(header + 4);

   141

}

   142

   143

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

   144

   145

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   146

{

   147

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

   148

}

   149

   150

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

   151

   152

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   153

{

   154

    return voe->data_size;

   155

}

   156

   157

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

   158

   159

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   160

{

   161

    voe->dir = EC_DIR_INPUT;

   162

    voe->state = ec_voe_handler_state_read_start;

   163

    voe->request_state = EC_INT_REQUEST_BUSY;

   164

}

   165

   166

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

   167

   168

void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)

   169

{

   170

    voe->dir = EC_DIR_INPUT;

   171

    voe->state = ec_voe_handler_state_read_nosync_start;

   172

    voe->request_state = EC_INT_REQUEST_BUSY;

   173

}

   174

   175

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

   176

   177

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   178

{

   179

    voe->dir = EC_DIR_OUTPUT;

   180

    voe->data_size = size;

   181

    voe->state = ec_voe_handler_state_write_start;

   182

    voe->request_state = EC_INT_REQUEST_BUSY;

   183

}

   184

   185

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

   186

   187

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   188

{

   189

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

   190

        voe->state(voe);

   191

        if (voe->request_state == EC_INT_REQUEST_BUSY) {

   192

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

   193

        }

   194

    } else {

   195

        voe->state = ec_voe_handler_state_error;

   196

        voe->request_state = EC_INT_REQUEST_FAILURE;

   197

    }

   198

   199

    return ec_request_state_translation_table[voe->request_state];

   200

}

   201

   202

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

   203

 * State functions.

   204

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

   205

   206

/** Start writing VoE data.

   207

 */

   208

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   209

{

   210

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

   211

    uint8_t *data;

   212

   213

    if (slave->master->debug_level) {

   214

        EC_SLAVE_DBG(slave, 0, "Writing %zu bytes of VoE data.\n",

   215

               voe->data_size);

   216

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

   217

    }

   218

   219

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

   220

        EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");

   221

        voe->state = ec_voe_handler_state_error;

   222

        voe->request_state = EC_INT_REQUEST_FAILURE;

   223

        return;

   224

    }

   225

   226

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

   227

            EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);

   228

    if (IS_ERR(data)) {

   229

        voe->state = ec_voe_handler_state_error;

   230

        voe->request_state = EC_INT_REQUEST_FAILURE;

   231

        return;

   232

    }

   233

   234

    EC_WRITE_U32(data,     voe->vendor_id);

   235

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

   236

    /* data already in datagram */

   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_SLAVE_ERR(slave, "Failed to receive VoE write request datagram: ");

   259

        ec_datagram_print_state(datagram);

   260

        return;

   261

    }

   262

   263

    if (datagram->working_counter != 1) {

   264

        if (!datagram->working_counter) {

   265

            unsigned long diff_ms =

   266

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

   267

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   268

                EC_SLAVE_DBG(slave, 1, "Slave did not respond to"

   269

                        " VoE write request. Retrying after %lu ms...\n",

   270

                        diff_ms);

   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_SLAVE_ERR(slave, "Reception of VoE write request failed: ");

   278

        ec_datagram_print_wc_error(datagram);

   279

        return;

   280

    }

   281

   282

    EC_CONFIG_DBG(voe->config, 1, "VoE write request successful.\n");

   283

   284

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   285

    voe->state = ec_voe_handler_state_end;

   286

}

   287

   288

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

   289

   290

/** Start reading VoE data.

   291

 */

   292

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   293

{

   294

    ec_datagram_t *datagram = &voe->datagram;

   295

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

   296

   297

    EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");

   298

   299

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

   300

        EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");

   301

        voe->state = ec_voe_handler_state_error;

   302

        voe->request_state = EC_INT_REQUEST_FAILURE;

   303

        return;

   304

    }

   305

   306

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

   307

   308

    voe->jiffies_start = jiffies;

   309

    voe->retries = EC_FSM_RETRIES;

   310

    voe->state = ec_voe_handler_state_read_check;

   311

}

   312

   313

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

   314

   315

/** Check for new data in the mailbox.

   316

 */

   317

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   318

{

   319

    ec_datagram_t *datagram = &voe->datagram;

   320

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

   321

   322

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

   323

        return;

   324

   325

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   326

        voe->state = ec_voe_handler_state_error;

   327

        voe->request_state = EC_INT_REQUEST_FAILURE;

   328

        EC_SLAVE_ERR(slave, "Failed to receive VoE mailbox check datagram: ");

   329

        ec_datagram_print_state(datagram);

   330

        return;

   331

    }

   332

   333

    if (datagram->working_counter != 1) {

   334

        voe->state = ec_voe_handler_state_error;

   335

        voe->request_state = EC_INT_REQUEST_FAILURE;

   336

        EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"

   337

                " datagram failed: ");

   338

        ec_datagram_print_wc_error(datagram);

   339

        return;

   340

    }

   341

   342

    if (!ec_slave_mbox_check(datagram)) {

   343

        unsigned long diff_ms =

   344

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

   345

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   346

            voe->state = ec_voe_handler_state_error;

   347

            voe->request_state = EC_INT_REQUEST_FAILURE;

   348

            EC_SLAVE_ERR(slave, "Timeout while waiting for VoE data.\n");

   349

            return;

   350

        }

   351

   352

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

   353

        voe->retries = EC_FSM_RETRIES;

   354

        return;

   355

    }

   356

   357

    // Fetch response

   358

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

   359

    voe->retries = EC_FSM_RETRIES;

   360

    voe->state = ec_voe_handler_state_read_response;

   361

}

   362

   363

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

   364

   365

/** Read the pending mailbox data.

   366

 */

   367

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   368

{

   369

    ec_datagram_t *datagram = &voe->datagram;

   370

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

   371

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

   372

    uint8_t *data, mbox_prot;

   373

    size_t rec_size;

   374

   375

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

   376

        return;

   377

   378

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   379

        voe->state = ec_voe_handler_state_error;

   380

        voe->request_state = EC_INT_REQUEST_FAILURE;

   381

        EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");

   382

        ec_datagram_print_state(datagram);

   383

        return;

   384

    }

   385

   386

    if (datagram->working_counter != 1) {

   387

        voe->state = ec_voe_handler_state_error;

   388

        voe->request_state = EC_INT_REQUEST_FAILURE;

   389

        EC_SLAVE_ERR(slave, "Reception of VoE read response failed: ");

   390

        ec_datagram_print_wc_error(datagram);

   391

        return;

   392

    }

   393

   394

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

   395

    if (IS_ERR(data)) {

   396

        voe->state = ec_voe_handler_state_error;

   397

        voe->request_state = EC_INT_REQUEST_FAILURE;

   398

        return;

   399

    }

   400

   401

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   402

        voe->state = ec_voe_handler_state_error;

   403

        voe->request_state = EC_INT_REQUEST_FAILURE;

   404

        EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"

   405

                " as response.\n", mbox_prot);

   406

        ec_print_data(data, rec_size);

   407

        return;

   408

    }

   409

   410

    if (rec_size < EC_VOE_HEADER_SIZE) {

   411

        voe->state = ec_voe_handler_state_error;

   412

        voe->request_state = EC_INT_REQUEST_FAILURE;

   413

        EC_SLAVE_ERR(slave, "Received VoE header is"

   414

                " incomplete (%zu bytes)!\n", rec_size);

   415

        return;

   416

    }

   417

   418

    if (master->debug_level) {

   419

        EC_CONFIG_DBG(voe->config, 0, "VoE data:\n");

   420

        ec_print_data(data, rec_size);

   421

    }

   422

   423

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   424

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   425

    voe->state = ec_voe_handler_state_end; // success

   426

}

   427

   428

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

   429

   430

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

   431

 */

   432

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)

   433

{

   434

    ec_datagram_t *datagram = &voe->datagram;

   435

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

   436

   437

    EC_SLAVE_DBG(slave, 1, "Reading VoE data.\n");

   438

   439

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

   440

        EC_SLAVE_ERR(slave, "Slave does not support VoE!\n");

   441

        voe->state = ec_voe_handler_state_error;

   442

        voe->request_state = EC_INT_REQUEST_FAILURE;

   443

        return;

   444

    }

   445

   446

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

   447

   448

    voe->jiffies_start = jiffies;

   449

    voe->retries = EC_FSM_RETRIES;

   450

    voe->state = ec_voe_handler_state_read_nosync_response;

   451

}

   452

   453

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

   454

   455

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

   456

 *  might lead to an empty reponse from the client.

   457

 */

   458

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)

   459

{

   460

    ec_datagram_t *datagram = &voe->datagram;

   461

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

   462

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

   463

    uint8_t *data, mbox_prot;

   464

    size_t rec_size;

   465

   466

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

   467

        return;

   468

   469

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   470

        voe->state = ec_voe_handler_state_error;

   471

        voe->request_state = EC_INT_REQUEST_FAILURE;

   472

        EC_SLAVE_ERR(slave, "Failed to receive VoE read datagram: ");

   473

        ec_datagram_print_state(datagram);

   474

        return;

   475

    }

   476

   477

    if (datagram->working_counter == 0) {

   478

        voe->state = ec_voe_handler_state_error;

   479

        voe->request_state = EC_INT_REQUEST_FAILURE;

   480

        EC_SLAVE_DBG(slave, 1, "Slave did not send VoE data.\n");

   481

        return;

   482

    }

   483

   484

    if (datagram->working_counter != 1) {

   485

        voe->state = ec_voe_handler_state_error;

   486

        voe->request_state = EC_INT_REQUEST_FAILURE;

   487

        EC_SLAVE_WARN(slave, "Reception of VoE read response failed: ");

   488

        ec_datagram_print_wc_error(datagram);

   489

        return;

   490

    }

   491

   492

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

   493

                    &mbox_prot, &rec_size))) {

   494

        voe->state = ec_voe_handler_state_error;

   495

        voe->request_state = EC_INT_REQUEST_FAILURE;

   496

        return;

   497

    }

   498

   499

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   500

        voe->state = ec_voe_handler_state_error;

   501

        voe->request_state = EC_INT_REQUEST_FAILURE;

   502

        EC_SLAVE_WARN(slave, "Received mailbox protocol 0x%02X"