1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/** \file

    35

 * Vendor-specific-over-EtherCAT protocol handler functions.

    36

 */

    37

    38

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

    39

    40

#include <linux/module.h>

    41

    42

#include "master.h"

    43

#include "slave_config.h"

    44

#include "mailbox.h"

    45

#include "voe_handler.h"

    46

    47

/** VoE mailbox type.

    48

 */

    49

#define EC_MBOX_TYPE_VOE 0x0f

    50

    51

/** VoE header size.

    52

 */

    53

#define EC_VOE_HEADER_SIZE 6

    54

    55

/** VoE response timeout in [ms].

    56

 */

    57

#define EC_VOE_RESPONSE_TIMEOUT 500

    58

    59

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

    60

    61

void ec_voe_handler_clear_data(ec_voe_handler_t *);

    62

    63

void ec_voe_handler_state_write_start(ec_voe_handler_t *);

    64

void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    65

    66

void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    67

void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    68

void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    69

    70

void ec_voe_handler_state_end(ec_voe_handler_t *);

    71

void ec_voe_handler_state_error(ec_voe_handler_t *);

    72

    73

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

    74

    75

/** VoE handler constructor.

    76

 */

    77

int ec_voe_handler_init(

    78

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

    79

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

    80

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

    81

        )

    82

{

    83

    voe->config = sc;

    84

    voe->vendor_id = 0x00000000;

    85

    voe->vendor_type = 0x0000;

    86

    voe->data_size = 0;

    87

    voe->dir = EC_DIR_INVALID;

    88

    voe->state = ec_voe_handler_state_error;

    89

    voe->request_state = EC_INT_REQUEST_INIT;

    90

    91

    ec_datagram_init(&voe->datagram);

    92

    if (ec_datagram_prealloc(&voe->datagram,

    93

                size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE))

    94

        return -1;

    95

    96

    return 0;

    97

}

    98

    99

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

   100

   101

/** VoE handler destructor.

   102

 */

   103

void ec_voe_handler_clear(

   104

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

   105

        )

   106

{

   107

    ec_datagram_clear(&voe->datagram);

   108

}

   109

   110

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

   111

 * Application interface.

   112

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

   113

   114

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   115

        uint16_t vendor_type)

   116

{

   117

    voe->vendor_id = vendor_id;

   118

    voe->vendor_type = vendor_type;

   119

}

   120

   121

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

   122

   123

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   124

        uint32_t *vendor_id, uint16_t *vendor_type)

   125

{

   126

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

   127

   128

    if (vendor_id)

   129

        *vendor_id = EC_READ_U32(header);

   130

    if (vendor_type)

   131

        *vendor_type = EC_READ_U16(header + 4);

   132

}

   133

   134

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

   135

   136

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   137

{

   138

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

   139

}

   140

   141

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

   142

   143

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   144

{

   145

    return voe->data_size;

   146

}

   147

   148

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

   149

   150

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   151

{

   152

    voe->dir = EC_DIR_INPUT;

   153

    voe->state = ec_voe_handler_state_read_start;

   154

    voe->request_state = EC_INT_REQUEST_QUEUED;

   155

}

   156

   157

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

   158

   159

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   160

{

   161

    voe->dir = EC_DIR_OUTPUT;

   162

    voe->data_size = size;

   163

    voe->state = ec_voe_handler_state_write_start;

   164

    voe->request_state = EC_INT_REQUEST_QUEUED;

   165

}

   166

   167

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

   168

   169

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   170

{

   171

    if (voe->config->slave) {

   172

        voe->state(voe);

   173

        if (voe->request_state == EC_REQUEST_BUSY)

   174

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

   175

    } else {

   176

        voe->state = ec_voe_handler_state_error;

   177

        voe->request_state = EC_INT_REQUEST_FAILURE;

   178

    }

   179

   180

    return ec_request_state_translation_table[voe->request_state];

   181

}

   182

   183

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

   184

 * State functions.

   185

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

   186

   187

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   188

{

   189

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

   190

    uint8_t *data;

   191

   192

    if (slave->master->debug_level) {

   193

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

   194

               voe->data_size, slave->ring_position);

   195

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

   196

    }

   197

   198

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

   199

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

   200

        voe->state = ec_voe_handler_state_error;

   201

        voe->request_state = EC_INT_REQUEST_FAILURE;

   202

        return;

   203

    }

   204

   205

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

   206

                    EC_MBOX_TYPE_VOE, EC_VOE_HEADER_SIZE + voe->data_size))) {

   207

        voe->state = ec_voe_handler_state_error;

   208

        voe->request_state = EC_INT_REQUEST_FAILURE;

   209

        return;

   210

    }

   211

   212

    EC_WRITE_U32(data,     voe->vendor_id);

   213

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

   214

   215

    voe->retries = EC_FSM_RETRIES;

   216

    voe->jiffies_start = jiffies;

   217

    voe->state = ec_voe_handler_state_write_response;

   218

}

   219

   220

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

   221

   222

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   223

{

   224

    ec_datagram_t *datagram = &voe->datagram;

   225

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

   226

   227

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

   228

        return;

   229

   230

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   231

        voe->state = ec_voe_handler_state_error;

   232

        voe->request_state = EC_INT_REQUEST_FAILURE;

   233

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

   234

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

   235

               slave->ring_position, datagram->state);

   236

        return;

   237

    }

   238

   239

    if (datagram->working_counter != 1) {

   240

        if (!datagram->working_counter) {

   241

            unsigned long diff_ms =

   242

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

   243

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   244

                if (slave->master->debug_level) {

   245

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

   246

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

   247

                            slave->ring_position, (u32) diff_ms);

   248

                }

   249

                // no response; send request datagram again

   250

                return;

   251

            }

   252

        }

   253

        voe->state = ec_voe_handler_state_error;

   254

        voe->request_state = EC_INT_REQUEST_FAILURE;

   255

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

   256

                slave->ring_position);

   257

        ec_datagram_print_wc_error(datagram);

   258

        return;

   259

    }

   260

   261

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

   262

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

   263

   264

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   265

    voe->state = ec_voe_handler_state_end;

   266

}

   267

   268

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

   269

   270

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   271

{

   272

    ec_datagram_t *datagram = &voe->datagram;

   273

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

   274

   275

    if (slave->master->debug_level)

   276

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

   277

   278

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

   279

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

   280

        voe->state = ec_voe_handler_state_error;

   281

        voe->request_state = EC_INT_REQUEST_FAILURE;

   282

        return;

   283

    }

   284

   285

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

   286

   287

    voe->jiffies_start = jiffies;

   288

    voe->retries = EC_FSM_RETRIES;

   289

    voe->state = ec_voe_handler_state_read_check;

   290

}

   291

   292

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

   293

   294

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   295

{

   296

    ec_datagram_t *datagram = &voe->datagram;

   297

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

   298

   299

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

   300

        return;

   301

   302

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   303

        voe->state = ec_voe_handler_state_error;

   304

        voe->request_state = EC_INT_REQUEST_FAILURE;

   305

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

   306

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

   307

               slave->ring_position, datagram->state);

   308

        return;

   309

    }

   310

   311

    if (datagram->working_counter != 1) {

   312

        voe->state = ec_voe_handler_state_error;

   313

        voe->request_state = EC_INT_REQUEST_FAILURE;

   314

        EC_ERR("Reception of VoE mailbox check"

   315

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

   316

        ec_datagram_print_wc_error(datagram);

   317

        return;

   318

    }

   319

   320

    if (!ec_slave_mbox_check(datagram)) {

   321

        unsigned long diff_ms =

   322

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

   323

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   324

            voe->state = ec_voe_handler_state_error;

   325

            voe->request_state = EC_INT_REQUEST_FAILURE;

   326

            EC_ERR("Timeout while waiting for VoE data on "

   327

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

   328

            return;

   329

        }

   330

   331

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

   332

        voe->retries = EC_FSM_RETRIES;

   333

        return;

   334

    }

   335

   336

    // Fetch response

   337

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

   338

    voe->retries = EC_FSM_RETRIES;

   339

    voe->state = ec_voe_handler_state_read_response;

   340

}

   341

   342

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

   343

   344

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   345

{

   346

    ec_datagram_t *datagram = &voe->datagram;

   347

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

   348

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

   349

    uint8_t *data, mbox_prot;

   350

    size_t rec_size;

   351

   352

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

   353

        return;

   354

   355

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   356

        voe->state = ec_voe_handler_state_error;

   357

        voe->request_state = EC_INT_REQUEST_FAILURE;

   358

        EC_ERR("Failed to receive VoE read datagram for"

   359

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

   360

               slave->ring_position, datagram->state);

   361

        return;

   362

    }

   363

   364

    if (datagram->working_counter != 1) {

   365

        voe->state = ec_voe_handler_state_error;

   366

        voe->request_state = EC_INT_REQUEST_FAILURE;

   367

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

   368

                slave->ring_position);

   369

        ec_datagram_print_wc_error(datagram);

   370

        return;

   371

    }

   372

   373

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

   374

				     &mbox_prot, &rec_size))) {

   375

        voe->state = ec_voe_handler_state_error;

   376

        voe->request_state = EC_INT_REQUEST_FAILURE;

   377

        return;

   378

    }

   379

   380

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   381

        voe->state = ec_voe_handler_state_error;

   382

        voe->request_state = EC_INT_REQUEST_FAILURE;

   383

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

   384

        ec_print_data(data, rec_size);

   385

        return;

   386

    }

   387

   388

    if (rec_size < EC_VOE_HEADER_SIZE) {

   389

        voe->state = ec_voe_handler_state_error;

   390

        voe->request_state = EC_INT_REQUEST_FAILURE;

   391

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

   392

        return;

   393

    }

   394

   395

    if (master->debug_level) {

   396

        EC_DBG("VoE data:\n");

   397

        ec_print_data(data, rec_size);

   398

    }

   399

   400

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   401

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   402

    voe->state = ec_voe_handler_state_end; // success

   403

}

   404

   405

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

   406

   407

void ec_voe_handler_state_end(ec_voe_handler_t *voe)

   408

{

   409

}

   410

   411

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

   412

   413

void ec_voe_handler_state_error(ec_voe_handler_t *voe)

   414

{

   415

}

   416

   417

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

   418

   419

/** \cond */

   420

   421

EXPORT_SYMBOL(ecrt_voe_handler_send_header);

   422

EXPORT_SYMBOL(ecrt_voe_handler_received_header);

   423

EXPORT_SYMBOL(ecrt_voe_handler_data);

   424

EXPORT_SYMBOL(ecrt_voe_handler_data_size);

   425

EXPORT_SYMBOL(ecrt_voe_handler_read);

   426

EXPORT_SYMBOL(ecrt_voe_handler_write);

   427

EXPORT_SYMBOL(ecrt_voe_handler_execute);

   428

   429

/** \endcond */

   430

   431

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