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

   112

/** Get usable memory size.

   113

 */

   114

size_t ec_voe_handler_mem_size(

   115

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

   116

		)

   117

{

   118

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

   119

		return voe->datagram.mem_size -

   120

			(EC_MBOX_HEADER_SIZE + EC_VOE_HEADER_SIZE);

   121

	else

   122

		return 0;

   123

}

   124

   125

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

   126

 * Application interface.

   127

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

   128

   129

void ecrt_voe_handler_send_header(ec_voe_handler_t *voe, uint32_t vendor_id,

   130

        uint16_t vendor_type)

   131

{

   132

    voe->vendor_id = vendor_id;

   133

    voe->vendor_type = vendor_type;

   134

}

   135

   136

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

   137

   138

void ecrt_voe_handler_received_header(const ec_voe_handler_t *voe,

   139

        uint32_t *vendor_id, uint16_t *vendor_type)

   140

{

   141

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

   142

   143

    if (vendor_id)

   144

        *vendor_id = EC_READ_U32(header);

   145

    if (vendor_type)

   146

        *vendor_type = EC_READ_U16(header + 4);

   147

}

   148

   149

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

   150

   151

uint8_t *ecrt_voe_handler_data(ec_voe_handler_t *voe)

   152

{

   153

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

   154

}

   155

   156

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

   157

   158

size_t ecrt_voe_handler_data_size(const ec_voe_handler_t *voe)

   159

{

   160

    return voe->data_size;

   161

}

   162

   163

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

   164

   165

void ecrt_voe_handler_read(ec_voe_handler_t *voe)

   166

{

   167

    voe->dir = EC_DIR_INPUT;

   168

    voe->state = ec_voe_handler_state_read_start;

   169

    voe->request_state = EC_INT_REQUEST_QUEUED;

   170

}

   171

   172

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

   173

   174

void ecrt_voe_handler_write(ec_voe_handler_t *voe, size_t size)

   175

{

   176

    voe->dir = EC_DIR_OUTPUT;

   177

    voe->data_size = size;

   178

    voe->state = ec_voe_handler_state_write_start;

   179

    voe->request_state = EC_INT_REQUEST_QUEUED;

   180

}

   181

   182

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

   183

   184

ec_request_state_t ecrt_voe_handler_execute(ec_voe_handler_t *voe)

   185

{

   186

    if (voe->config->slave) {

   187

        voe->state(voe);

   188

        if (voe->request_state == EC_REQUEST_BUSY)

   189

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

   190

    } else {

   191

        voe->state = ec_voe_handler_state_error;

   192

        voe->request_state = EC_INT_REQUEST_FAILURE;

   193

    }

   194

   195

    return ec_request_state_translation_table[voe->request_state];

   196

}

   197

   198

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

   199

 * State functions.

   200

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

   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

void ec_voe_handler_state_write_response(ec_voe_handler_t *voe)

   238

{

   239

    ec_datagram_t *datagram = &voe->datagram;

   240

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

   241

   242

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

   243

        return;

   244

   245

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   246

        voe->state = ec_voe_handler_state_error;

   247

        voe->request_state = EC_INT_REQUEST_FAILURE;

   248

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

   249

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

   250

               slave->ring_position, datagram->state);

   251

        return;

   252

    }

   253

   254

    if (datagram->working_counter != 1) {

   255

        if (!datagram->working_counter) {

   256

            unsigned long diff_ms =

   257

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

   258

            if (diff_ms < EC_VOE_RESPONSE_TIMEOUT) {

   259

                if (slave->master->debug_level) {

   260

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

   261

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

   262

                            slave->ring_position, (u32) diff_ms);

   263

                }

   264

                // no response; send request datagram again

   265

                return;

   266

            }

   267

        }

   268

        voe->state = ec_voe_handler_state_error;

   269

        voe->request_state = EC_INT_REQUEST_FAILURE;

   270

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

   271

                slave->ring_position);

   272

        ec_datagram_print_wc_error(datagram);

   273

        return;

   274

    }

   275

   276

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

   277

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

   278

   279

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   280

    voe->state = ec_voe_handler_state_end;

   281

}

   282

   283

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

   284

   285

void ec_voe_handler_state_read_start(ec_voe_handler_t *voe)

   286

{

   287

    ec_datagram_t *datagram = &voe->datagram;

   288

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

   289

   290

    if (slave->master->debug_level)

   291

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

   292

   293

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

   294

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

   295

        voe->state = ec_voe_handler_state_error;

   296

        voe->request_state = EC_INT_REQUEST_FAILURE;

   297

        return;

   298

    }

   299

   300

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

   301

   302

    voe->jiffies_start = jiffies;

   303

    voe->retries = EC_FSM_RETRIES;

   304

    voe->state = ec_voe_handler_state_read_check;

   305

}

   306

   307

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

   308

   309

void ec_voe_handler_state_read_check(ec_voe_handler_t *voe)

   310

{

   311

    ec_datagram_t *datagram = &voe->datagram;

   312

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

   313

   314

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

   315

        return;

   316

   317

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   318

        voe->state = ec_voe_handler_state_error;

   319

        voe->request_state = EC_INT_REQUEST_FAILURE;

   320

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

   321

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

   322

               slave->ring_position, datagram->state);

   323

        return;

   324

    }

   325

   326

    if (datagram->working_counter != 1) {

   327

        voe->state = ec_voe_handler_state_error;

   328

        voe->request_state = EC_INT_REQUEST_FAILURE;

   329

        EC_ERR("Reception of VoE mailbox check"

   330

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

   331

        ec_datagram_print_wc_error(datagram);

   332

        return;

   333

    }

   334

   335

    if (!ec_slave_mbox_check(datagram)) {

   336

        unsigned long diff_ms =

   337

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

   338

        if (diff_ms >= EC_VOE_RESPONSE_TIMEOUT) {

   339

            voe->state = ec_voe_handler_state_error;

   340

            voe->request_state = EC_INT_REQUEST_FAILURE;

   341

            EC_ERR("Timeout while waiting for VoE data on "

   342

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

   343

            return;

   344

        }

   345

   346

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

   347

        voe->retries = EC_FSM_RETRIES;

   348

        return;

   349

    }

   350

   351

    // Fetch response

   352

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

   353

    voe->retries = EC_FSM_RETRIES;

   354

    voe->state = ec_voe_handler_state_read_response;

   355

}

   356

   357

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

   358

   359

void ec_voe_handler_state_read_response(ec_voe_handler_t *voe)

   360

{

   361

    ec_datagram_t *datagram = &voe->datagram;

   362

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

   363

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

   364

    uint8_t *data, mbox_prot;

   365

    size_t rec_size;

   366

   367

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

   368

        return;

   369

   370

    if (datagram->state != EC_DATAGRAM_RECEIVED) {

   371

        voe->state = ec_voe_handler_state_error;

   372

        voe->request_state = EC_INT_REQUEST_FAILURE;

   373

        EC_ERR("Failed to receive VoE read datagram for"

   374

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

   375

               slave->ring_position, datagram->state);

   376

        return;

   377

    }

   378

   379

    if (datagram->working_counter != 1) {

   380

        voe->state = ec_voe_handler_state_error;

   381

        voe->request_state = EC_INT_REQUEST_FAILURE;

   382

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

   383

                slave->ring_position);

   384

        ec_datagram_print_wc_error(datagram);

   385

        return;

   386

    }

   387

   388

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

   389

				     &mbox_prot, &rec_size))) {

   390

        voe->state = ec_voe_handler_state_error;

   391

        voe->request_state = EC_INT_REQUEST_FAILURE;

   392

        return;

   393

    }

   394

   395

    if (mbox_prot != EC_MBOX_TYPE_VOE) {

   396

        voe->state = ec_voe_handler_state_error;

   397

        voe->request_state = EC_INT_REQUEST_FAILURE;

   398

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

   399

        ec_print_data(data, rec_size);

   400

        return;

   401

    }

   402

   403

    if (rec_size < EC_VOE_HEADER_SIZE) {

   404

        voe->state = ec_voe_handler_state_error;

   405

        voe->request_state = EC_INT_REQUEST_FAILURE;

   406

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

   407

        return;

   408

    }

   409

   410

    if (master->debug_level) {

   411

        EC_DBG("VoE data:\n");

   412

        ec_print_data(data, rec_size);

   413

    }

   414

   415

    voe->data_size = rec_size - EC_VOE_HEADER_SIZE;

   416

    voe->request_state = EC_INT_REQUEST_SUCCESS;

   417

    voe->state = ec_voe_handler_state_end; // success

   418

}

   419

   420

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

   421

   422

void ec_voe_handler_state_end(ec_voe_handler_t *voe)

   423

{

   424

}

   425

   426

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

   427

   428

void ec_voe_handler_state_error(ec_voe_handler_t *voe)

   429

{

   430

}

   431

   432

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

   433

   434

/** \cond */

   435

   436

EXPORT_SYMBOL(ecrt_voe_handler_send_header);

   437

EXPORT_SYMBOL(ecrt_voe_handler_received_header);

   438

EXPORT_SYMBOL(ecrt_voe_handler_data);

   439

EXPORT_SYMBOL(ecrt_voe_handler_data_size);

   440

EXPORT_SYMBOL(ecrt_voe_handler_read);

   441

EXPORT_SYMBOL(ecrt_voe_handler_write);

   442

EXPORT_SYMBOL(ecrt_voe_handler_execute);

   443

   444

/** \endcond */

   445

   446

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