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

    ec_mbox_init(&voe->mbox,&voe->datagram);

    92

    return ec_datagram_prealloc(&voe->datagram,

    93

            size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

    94

}

    95

    96

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

    97

    98

/** VoE handler destructor.

    99

 */

   100

void ec_voe_handler_clear(

   101

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

   102

        )

   103

{

   104

    ec_mbox_clear(&voe->mbox);

   105

    ec_datagram_clear(&voe->datagram);

   106

}

   107

   108

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

   109

   110

/** Get usable memory size.

   111

 */

   112

size_t ec_voe_handler_mem_size(

   113

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

   114

        )

   115

{

   116

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

   117

        return voe->datagram.mem_size -

   118

            (EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

   119

    else

   120

        return 0;

   121

}

   122

   123

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

   124

 * Application interface.

   125

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

   126

   127

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   128

        uint16_t vendor_type)

   129

{

   130

    voe->vendor_id = vendor_id;

   131

    voe->vendor_type = vendor_type;

   132

}

   133

   134

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

   135

   136

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   137

        uint32_t *vendor_id, uint16_t *vendor_type)

   138

{

   139

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

   140

   141

    if (vendor_id)

   142

        *vendor_id = EC_READ_U32(header);

   143

    if (vendor_type)

   144

        *vendor_type = EC_READ_U16(header + 4);

   145

}

   146

   147

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

   148

   149

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   150

{

   151

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

   152

}

   153

   154

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

   155

   156

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   157

{

   158

    return voe->data_size;

   159

}

   160

   161

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

   162

   163

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   164

{

   165

    voe->dir = EC_DIR_INPUT;

   166

    voe->state = ec_voe_handler_state_read_start;

   167

    voe->request_state = EC_INT_REQUEST_BUSY;

   168

}

   169

   170

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

   171

   172

void ecrt_voe_handler_read_nosync(ec_voe_handler_t *voe)

   173

{

   174

    voe->dir = EC_DIR_INPUT;

   175

    voe->state = ec_voe_handler_state_read_nosync_start;

   176

    voe->request_state = EC_INT_REQUEST_BUSY;

   177

}

   178

   179

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

   180

   181

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   182

{

   183

    voe->dir = EC_DIR_OUTPUT;

   184

    voe->data_size = size;

   185

    voe->state = ec_voe_handler_state_write_start;

   186

    voe->request_state = EC_INT_REQUEST_BUSY;

   187

}

   188

   189

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

   190

   191

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   192

{

   193

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

   194

        voe->state(voe);

   195

        if (voe->request_state == EC_INT_REQUEST_BUSY)

   196

            ec_slave_mbox_queue_datagrams(voe->config->slave,&voe->mbox);

   197

    } else {

   198

        voe->state = ec_voe_handler_state_error;

   199

        voe->request_state = EC_INT_REQUEST_FAILURE;

   200

    }

   201

   202

    return ec_request_state_translation_table[voe->request_state];

   203

}

   204

   205

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

   206

 * State functions.

   207

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

   208

   209

/** Start writing VoE data.

   210

 */

   211

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   212

{

   213

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

   214

    uint8_t *data;

   215

   216

    if (slave->master->debug_level) {

   217

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

   218

               voe->data_size);

   219

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

   220

    }

   221

   222

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

   223

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

   224

        voe->state = ec_voe_handler_state_error;

   225

        voe->request_state = EC_INT_REQUEST_FAILURE;

   226

        return;

   227

    }

   228

   229

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

   230

            EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size);

   231

    if (IS_ERR(data)) {

   232

        voe->state = ec_voe_handler_state_error;

   233

        voe->request_state = EC_INT_REQUEST_FAILURE;

   234

        return;

   235

    }

   236

   237

    EC_WRITE_U32(data,     voe->vendor_id);

   238

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

   239

    /* data already in datagram */

   240

   241

    voe->retries = EC_FSM_RETRIES;

   242

    voe->jiffies_start = jiffies;

   243

    voe->state = ec_voe_handler_state_write_response;

   244

}

   245

   246

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

   247

   248

/** Wait for the mailbox response.

   249

 */

   250

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   251

{

   252

    ec_mailbox_t *mbox = &voe->mbox;

   253

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

   254

   255

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)

   256

        return;

   257

   258

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   259

        voe->state = ec_voe_handler_state_error;

   260

        voe->request_state = EC_INT_REQUEST_FAILURE;

   261

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

   262

        ec_datagram_print_state(mbox->datagram);

   263

        return;

   264

    }

   265

   266

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   267

        if (ec_mbox_is_datagram_wc(mbox,0)) {

   268

            unsigned long diff_ms =

   269

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

   270

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   271

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

   272

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

   273

                        diff_ms);

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

   281

        ec_datagram_print_wc_error(mbox->datagram);

   282

        return;

   283

    }

   284

   285

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

   286

   287

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   288

    voe->state = ec_voe_handler_state_end;

   289

}

   290

   291

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

   292

   293

/** Start reading VoE data.

   294

 */

   295

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   296

{

   297

    ec_mailbox_t *mbox = &voe->mbox;

   298

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

   299

   300

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

   301

   302

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

   303

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

   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, mbox); // 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_mailbox_t *mbox = &voe->mbox;

   323

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

   324

   325

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)

   326

        return;

   327

   328

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   329

        voe->state = ec_voe_handler_state_error;

   330

        voe->request_state = EC_INT_REQUEST_FAILURE;

   331

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

   332

        ec_datagram_print_state(mbox->datagram);

   333

        return;

   334

    }

   335

   336

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   337

        voe->state = ec_voe_handler_state_error;

   338

        voe->request_state = EC_INT_REQUEST_FAILURE;

   339

        EC_SLAVE_ERR(slave, "Reception of VoE mailbox check"

   340

                " datagram failed: ");

   341

        ec_datagram_print_wc_error(mbox->datagram);

   342

        return;

   343

    }

   344

   345

    if (!ec_slave_mbox_check(mbox)) {

   346

        unsigned long diff_ms =

   347

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

   348

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   349

            voe->state = ec_voe_handler_state_error;

   350

            voe->request_state = EC_INT_REQUEST_FAILURE;

   351

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

   352

            return;

   353

        }

   354

   355

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

   356

        voe->retries = EC_FSM_RETRIES;

   357

        return;

   358

    }

   359

   360

    // Fetch response

   361

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

   362

    voe->retries = EC_FSM_RETRIES;

   363

    voe->state = ec_voe_handler_state_read_response;

   364

}

   365

   366

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

   367

   368

/** Read the pending mailbox data.

   369

 */

   370

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   371

{

   372

    ec_mailbox_t *mbox = &voe->mbox;

   373

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

   374

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

   375

    uint8_t *data, mbox_prot;

   376

    size_t rec_size;

   377

   378

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)

   379

        return;

   380

   381

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   382

        voe->state = ec_voe_handler_state_error;

   383

        voe->request_state = EC_INT_REQUEST_FAILURE;

   384

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

   385

        ec_datagram_print_state(mbox->datagram);

   386

        return;

   387

    }

   388

   389

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   390

        voe->state = ec_voe_handler_state_error;

   391

        voe->request_state = EC_INT_REQUEST_FAILURE;

   392

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

   393

        ec_datagram_print_wc_error(mbox->datagram);

   394

        return;

   395

    }

   396

   397

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

   398

    if (IS_ERR(data)) {

   399

        voe->state = ec_voe_handler_state_error;

   400

        voe->request_state = EC_INT_REQUEST_FAILURE;

   401

        return;

   402

    }

   403

   404

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   405

        voe->state = ec_voe_handler_state_error;

   406

        voe->request_state = EC_INT_REQUEST_FAILURE;

   407

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

   408

                " as response.\n", mbox_prot);

   409

        ec_print_data(data, rec_size);

   410

        return;

   411

    }

   412

   413

    if (rec_size < EC_VOE_HEADER_SIZE) {

   414

        voe->state = ec_voe_handler_state_error;

   415

        voe->request_state = EC_INT_REQUEST_FAILURE;

   416

        EC_SLAVE_ERR(slave, "Received VoE header is"

   417

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

   418

        return;

   419

    }

   420

   421

    if (master->debug_level) {

   422

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

   423

        ec_print_data(data, rec_size);

   424

    }

   425

   426

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   427

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   428

    voe->state = ec_voe_handler_state_end; // success

   429

}

   430

   431

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

   432

   433

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

   434

 */

   435

void ec_voe_handler_state_read_nosync_start(ec_voe_handler_t *voe)

   436

{

   437

    ec_mailbox_t *mbox = &voe->mbox;

   438

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

   439

   440

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

   441

   442

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

   443

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

   444

        voe->state = ec_voe_handler_state_error;

   445

        voe->request_state = EC_INT_REQUEST_FAILURE;

   446

        return;

   447

    }

   448

   449

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

   450

   451

    voe->jiffies_start = jiffies;

   452

    voe->retries = EC_FSM_RETRIES;

   453

    voe->state = ec_voe_handler_state_read_nosync_response;

   454

}

   455

   456

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

   457

   458

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

   459

 *  might lead to an empty reponse from the client.

   460

 */

   461

void ec_voe_handler_state_read_nosync_response(ec_voe_handler_t *voe)

   462

{

   463

    ec_mailbox_t *mbox = &voe->mbox;

   464

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

   465

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

   466

    uint8_t *data, mbox_prot;

   467

    size_t rec_size;

   468

   469

    if (ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_TIMED_OUT) && voe->retries--)

   470

        return;

   471

   472

    if (!ec_mbox_is_datagram_state(mbox,EC_DATAGRAM_RECEIVED)) {

   473

        voe->state = ec_voe_handler_state_error;

   474

        voe->request_state = EC_INT_REQUEST_FAILURE;

   475

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

   476

        ec_datagram_print_state(mbox->datagram);

   477

        return;

   478

    }

   479

   480

    if (ec_mbox_is_datagram_wc(mbox,0)) {

   481

        voe->state = ec_voe_handler_state_error;

   482

        voe->request_state = EC_INT_REQUEST_FAILURE;

   483

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

   484

        return;

   485

    }

   486

   487

    if (!ec_mbox_is_datagram_wc(mbox,1)) {

   488

        voe->state = ec_voe_handler_state_error;

   489

        voe->request_state = EC_INT_REQUEST_FAILURE;

   490

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

   491

        ec_datagram_print_wc_error(mbox->datagram);

   492

        return;

   493

    }

   494

   495

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

   496

                    &mbox_prot, &rec_size))) {

   497

        voe->state = ec_voe_handler_state_error;

   498

        voe->request_state = EC_INT_REQUEST_FAILURE;

   499

        return;

   500

    }

   501

   502