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_state_write_start(ec_voe_handler_t *);

    62

void ec_voe_handler_state_write_response(ec_voe_handler_t *);

    63

    64

void ec_voe_handler_state_read_start(ec_voe_handler_t *);

    65

void ec_voe_handler_state_read_check(ec_voe_handler_t *);

    66

void ec_voe_handler_state_read_response(ec_voe_handler_t *);

    67

    68

void ec_voe_handler_state_end(ec_voe_handler_t *);

    69

void ec_voe_handler_state_error(ec_voe_handler_t *);

    70

    71

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

    72

    73

/** VoE handler constructor.

    74

 */

    75

int ec_voe_handler_init(

    76

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

    77

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

    78

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

    79

        )

    80

{

    81

    voe->config = sc;

    82

    voe->vendor_id = 0x00000000;

    83

    voe->vendor_type = 0x0000;

    84

    voe->data_size = 0;

    85

    voe->dir = EC_DIR_INVALID;

    86

    voe->state = ec_voe_handler_state_error;

    87

    voe->request_state = EC_INT_REQUEST_INIT;

    88

    89

    ec_datagram_init(&voe->datagram);

    90

    if (ec_datagram_prealloc(&voe->datagram,

    91

                size + EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE))

    92

        return -1;

    93

    94

    return 0;

    95

}

    96

    97

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

    98

    99

/** VoE handler destructor.

   100

 */

   101

void ec_voe_handler_clear(

   102

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

   103

        )

   104

{

   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_QUEUED;

   168

}

   169

   170

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

   171

   172

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   173

{

   174

    voe->dir = EC_DIR_OUTPUT;

   175

    voe->data_size = size;

   176

    voe->state = ec_voe_handler_state_write_start;

   177

    voe->request_state = EC_INT_REQUEST_QUEUED;

   178

}

   179

   180

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

   181

   182

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   183

{

   184

    if (voe->config->slave) {

   185

        voe->state(voe);

   186

        if (voe->request_state == EC_REQUEST_BUSY)

   187

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

   188

    } else {

   189

        voe->state = ec_voe_handler_state_error;

   190

        voe->request_state = EC_INT_REQUEST_FAILURE;

   191

    }

   192

   193

    return ec_request_state_translation_table[voe->request_state];

   194

}

   195

   196

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

   197

 * State functions.

   198

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

   199

   200

/** Start writing VoE data.

   201

 */

   202

void ec_voe_handler_state_write_start(ec_voe_handler_t *voe)

   203

{

   204

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

   205

    uint8_t *data;

   206

   207

    if (slave->master->debug_level) {

   208

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

   209

               voe->data_size, slave->ring_position);

   210

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

   211

    }

   212

   213

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

   214

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

   215

        voe->state = ec_voe_handler_state_error;

   216

        voe->request_state = EC_INT_REQUEST_FAILURE;

   217

        return;

   218

    }

   219

   220

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

   221

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

   222

        voe->state = ec_voe_handler_state_error;

   223

        voe->request_state = EC_INT_REQUEST_FAILURE;

   224

        return;

   225

    }

   226

   227

    EC_WRITE_U32(data,     voe->vendor_id);

   228

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

   229

   230

    voe->retries = EC_FSM_RETRIES;

   231

    voe->jiffies_start = jiffies;

   232

    voe->state = ec_voe_handler_state_write_response;

   233

}

   234

   235

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

   236

   237

/** Wait for the mailbox response.

   238

 */

   239

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   240

{

   241

    ec_datagram_t *datagram = &voe->datagram;

   242

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

   243

   244

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

   245

        return;

   246

   247

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   248

        voe->state = ec_voe_handler_state_error;

   249

        voe->request_state = EC_INT_REQUEST_FAILURE;

   250

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

   251

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

   252

               slave->ring_position, datagram->state);

   253

        return;

   254

    }

   255

   256

    if (datagram->working_counter != 1) {

   257

        if (!datagram->working_counter) {

   258

            unsigned long diff_ms =

   259

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

   260

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   261

                if (slave->master->debug_level) {

   262

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

   263

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

   264

                            slave->ring_position, (u32) diff_ms);

   265

                }

   266

                // no response; send request datagram again

   267

                return;

   268

            }

   269

        }

   270

        voe->state = ec_voe_handler_state_error;

   271

        voe->request_state = EC_INT_REQUEST_FAILURE;

   272

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

   273

                slave->ring_position);

   274

        ec_datagram_print_wc_error(datagram);

   275

        return;

   276

    }

   277

   278

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

   279

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

   280

   281

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   282

    voe->state = ec_voe_handler_state_end;

   283

}

   284

   285

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

   286

   287

/** Start reading VoE data.

   288

 */

   289

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   290

{

   291

    ec_datagram_t *datagram = &voe->datagram;

   292

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

   293

   294

    if (slave->master->debug_level)

   295

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

   296

   297

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

   298

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

   299

        voe->state = ec_voe_handler_state_error;

   300

        voe->request_state = EC_INT_REQUEST_FAILURE;

   301

        return;

   302

    }

   303

   304

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

   305

   306

    voe->jiffies_start = jiffies;

   307

    voe->retries = EC_FSM_RETRIES;

   308

    voe->state = ec_voe_handler_state_read_check;

   309

}

   310

   311

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

   312

   313

/** Check for new data in the mailbox.

   314

 */

   315

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   316

{

   317

    ec_datagram_t *datagram = &voe->datagram;

   318

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

   319

   320

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

   321

        return;

   322

   323

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   324

        voe->state = ec_voe_handler_state_error;

   325

        voe->request_state = EC_INT_REQUEST_FAILURE;

   326

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

   327

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

   328

               slave->ring_position, datagram->state);

   329

        return;

   330

    }

   331

   332

    if (datagram->working_counter != 1) {

   333

        voe->state = ec_voe_handler_state_error;

   334

        voe->request_state = EC_INT_REQUEST_FAILURE;

   335

        EC_ERR("Reception of VoE mailbox check"

   336

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

   337

        ec_datagram_print_wc_error(datagram);

   338

        return;

   339

    }

   340

   341

    if (!ec_slave_mbox_check(datagram)) {

   342

        unsigned long diff_ms =

   343

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

   344

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   345

            voe->state = ec_voe_handler_state_error;

   346

            voe->request_state = EC_INT_REQUEST_FAILURE;

   347

            EC_ERR("Timeout while waiting for VoE data on "

   348

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

   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_ERR("Failed to receive VoE read datagram for"

   382

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

   383

               slave->ring_position, datagram->state);

   384

        return;

   385

    }

   386

   387

    if (datagram->working_counter != 1) {

   388

        voe->state = ec_voe_handler_state_error;

   389

        voe->request_state = EC_INT_REQUEST_FAILURE;

   390

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

   391

                slave->ring_position);

   392

        ec_datagram_print_wc_error(datagram);

   393

        return;

   394

    }

   395

   396

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

   397

				     &mbox_prot, &rec_size))) {

   398

        voe->state = ec_voe_handler_state_error;

   399

        voe->request_state = EC_INT_REQUEST_FAILURE;

   400

        return;

   401

    }

   402

   403

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   404

        voe->state = ec_voe_handler_state_error;

   405

        voe->request_state = EC_INT_REQUEST_FAILURE;

   406

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

   407

        ec_print_data(data, rec_size);

   408

        return;

   409

    }

   410

   411

    if (rec_size < EC_VOE_HEADER_SIZE) {

   412

        voe->state = ec_voe_handler_state_error;

   413

        voe->request_state = EC_INT_REQUEST_FAILURE;

   414

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

   415

        return;

   416

    }

   417

   418

    if (master->debug_level) {

   419

        EC_DBG("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

/** Successful termination state function.

   431

 */

   432

void ec_voe_handler_state_end(ec_voe_handler_t *voe)

   433

{

   434

}

   435

   436

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

   437

   438

/** Failure termination state function.

   439

 */

   440

void ec_voe_handler_state_error(ec_voe_handler_t *voe)

   441

{

   442

}

   443

   444

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

   445

   446

/** \cond */

   447

   448

EXPORT_SYMBOL(ecrt_voe_handler_send_header);

   449

EXPORT_SYMBOL(ecrt_voe_handler_received_header);

   450

EXPORT_SYMBOL(ecrt_voe_handler_data);

   451

EXPORT_SYMBOL(ecrt_voe_handler_data_size);

   452

EXPORT_SYMBOL(ecrt_voe_handler_read);

   453

EXPORT_SYMBOL(ecrt_voe_handler_write);

   454

EXPORT_SYMBOL(ecrt_voe_handler_execute);

   455

   456

/** \endcond */

   457

   458

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