1

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

     2

 *

     3

 *  m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/module.h>

    12

#include <linux/kernel.h>

    13

#include <linux/string.h>

    14

#include <linux/slab.h>

    15

#include <linux/delay.h>

    16

    17

#include "../include/EtherCAT_rt.h"

    18

#include "globals.h"

    19

#include "master.h"

    20

#include "slave.h"

    21

#include "types.h"

    22

#include "device.h"

    23

#include "command.h"

    24

    25

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

    26

    27

// Prototypen

    28

    29

int ec_simple_send(ec_master_t *, ec_command_t *);

    30

int ec_simple_receive(ec_master_t *, ec_command_t *);

    31

void ec_output_debug_data(const ec_master_t *);

    32

int ec_sii_read(ec_master_t *, unsigned short, unsigned short, unsigned int *);

    33

void ec_output_lost_frames(ec_master_t *);

    34

    35

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

    36

    37

/**

    38

   Konstruktor des EtherCAT-Masters.

    39

    40

   @param master Zeiger auf den zu initialisierenden EtherCAT-Master

    41

*/

    42

    43

void ec_master_init(ec_master_t *master)

    44

{

    45

  master->bus_slaves = NULL;

    46

  master->bus_slaves_count = 0;

    47

  master->device_registered = 0;

    48

  master->command_index = 0x00;

    49

  master->tx_data_length = 0;

    50

  master->rx_data_length = 0;

    51

  master->domain_count = 0;

    52

  master->debug_level = 0;

    53

  master->bus_time = 0;

    54

  master->frames_lost = 0;

    55

  master->t_lost_output = 0;

    56

}

    57

    58

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

    59

    60

/**

    61

   Destruktor eines EtherCAT-Masters.

    62

    63

   Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    64

   auf das Slave-Array und gibt die Prozessdaten frei.

    65

    66

   @param master Zeiger auf den zu löschenden Master

    67

*/

    68

    69

void ec_master_clear(ec_master_t *master)

    70

{

    71

  if (master->bus_slaves) {

    72

    kfree(master->bus_slaves);

    73

    master->bus_slaves = NULL;

    74

  }

    75

    76

  ec_device_clear(&master->device);

    77

    78

  master->domain_count = 0;

    79

}

    80

    81

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

    82

    83

/**

   113

   Öffnet das EtherCAT-Geraet des Masters.

   114

   115

   @param master Der EtherCAT-Master

   116

   117

   @return 0, wenn alles o.k., < 0, wenn das Geraet nicht geoeffnet werden

   118

           konnte.

   119

*/

   120

   121

int ec_master_open(ec_master_t *master)

   122

{

   123

  if (!master->device_registered) {

   124

    printk(KERN_ERR "EtherCAT: No device registered!\n");

   125

    return -1;

   126

  }

   127

   128

  if (ec_device_open(&master->device) < 0) {

   129

    printk(KERN_ERR "EtherCAT: Could not open device!\n");

   130

    return -1;

   131

  }

   132

   133

  return 0;

   134

}

   135

   136

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

   137

   138

/**

   139

   Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   140

   141

   @param master Der EtherCAT-Master

   142

   @param device Das EtherCAT-Geraet

   143

*/

   144

   145

void ec_master_close(ec_master_t *master)

   146

{

   147

  if (!master->device_registered) {

   148

    printk(KERN_WARNING "EtherCAT: Warning -"

   149

           " Trying to close an unregistered device!\n");

   150

    return;

   151

  }

   152

   153

  if (ec_device_close(&master->device) < 0) {

   154

    printk(KERN_WARNING "EtherCAT: Warning - Could not close device!\n");

   155

  }

   156

}

   157

   158

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

   159

   160

/**

   161

   Sendet ein einzelnes Kommando in einem Frame und

   162

   wartet auf dessen Empfang.

   163

   164

   @param master EtherCAT-Master

   165

   @param cmd    Kommando zum Senden/Empfangen

   166

   167

   @return 0 bei Erfolg, sonst < 0

   168

*/

   169

   170

int ec_simple_send_receive(ec_master_t *master, ec_command_t *cmd)

   171

{

   172

  unsigned int tries_left;

   173

   174

  if (unlikely(ec_simple_send(master, cmd) < 0))

   175

    return -1;

   176

   177

  tries_left = 20;

   178

   179

  do

   180

  {

   181

    udelay(1);

   182

    ec_device_call_isr(&master->device);

   183

    tries_left--;

   184

  }

   185

  while (unlikely(master->device.state == EC_DEVICE_STATE_SENT && tries_left));

   186

   187

  if (unlikely(ec_simple_receive(master, cmd) < 0))

   188

    return -1;

   189

   190

  return 0;

   191

}

   192

   193

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

   194

   195

/**

   196

   Sendet ein einzelnes Kommando in einem Frame.

   197

   198

   @param master EtherCAT-Master

   199

   @param cmd    Kommando zum Senden

   200

   201

   @return 0 bei Erfolg, sonst < 0

   202

*/

   203

   204

int ec_simple_send(ec_master_t *master, ec_command_t *cmd)

   205

{

   206

  unsigned int length, framelength, i;

   207

   208

  if (unlikely(master->debug_level > 0)) {

   209

    printk(KERN_DEBUG "EtherCAT: ec_simple_send\n");

   210

  }

   211

   212

  if (unlikely(cmd->state != EC_COMMAND_STATE_READY)) {

   213

    printk(KERN_WARNING "EtherCAT: cmd not in ready state!\n");

   214

  }

   215

   216

  length = cmd->data_length + 12;

   217

  framelength = length + 2;

   218

   219

  if (unlikely(framelength > EC_FRAME_SIZE)) {

   220

    printk(KERN_ERR "EtherCAT: Frame too long (%i)!\n", framelength);

   221

    return -1;

   222

  }

   223

   224

  if (framelength < 46) framelength = 46;

   225

   226

  if (unlikely(master->debug_level > 0)) {

   227

    printk(KERN_DEBUG "EtherCAT: Frame length: %i\n", framelength);

   228

  }

   229

   230

  master->tx_data[0] = length & 0xFF;

   231

  master->tx_data[1] = ((length & 0x700) >> 8) | 0x10;

   232

   233

  cmd->index = master->command_index;

   234

  master->command_index = (master->command_index + 1) % 0x0100;

   235

   236

  if (unlikely(master->debug_level > 0)) {

   237

    printk(KERN_DEBUG "EtherCAT: Sending command index %i\n", cmd->index);

   238

  }

   239

   240

  cmd->state = EC_COMMAND_STATE_SENT;

   241

   242

  master->tx_data[2 + 0] = cmd->type;

   243

  master->tx_data[2 + 1] = cmd->index;

   244

  master->tx_data[2 + 2] = cmd->address.raw[0];

   245

  master->tx_data[2 + 3] = cmd->address.raw[1];

   246

  master->tx_data[2 + 4] = cmd->address.raw[2];

   247

  master->tx_data[2 + 5] = cmd->address.raw[3];

   248

  master->tx_data[2 + 6] = cmd->data_length & 0xFF;

   249

  master->tx_data[2 + 7] = (cmd->data_length & 0x700) >> 8;

   250

  master->tx_data[2 + 8] = 0x00;

   251

  master->tx_data[2 + 9] = 0x00;

   252

   253

  if (likely(cmd->type == EC_COMMAND_APWR

   254

             || cmd->type == EC_COMMAND_NPWR

   255

             || cmd->type == EC_COMMAND_BWR

   256

             || cmd->type == EC_COMMAND_LRW)) // Write commands

   257

  {

   258

    for (i = 0; i < cmd->data_length; i++)

   259

      master->tx_data[2 + 10 + i] = cmd->data[i];

   260

  }

   261

  else // Read commands

   262

  {

   263

    for (i = 0; i < cmd->data_length; i++) master->tx_data[2 + 10 + i] = 0x00;

   264

  }

   265

   266

  master->tx_data[2 + 10 + cmd->data_length] = 0x00;

   267

  master->tx_data[2 + 11 + cmd->data_length] = 0x00;

   268

   269

  // Pad with zeros

   270

  for (i = cmd->data_length + 12 + 2; i < 46; i++) master->tx_data[i] = 0x00;

   271

   272

  master->tx_data_length = framelength;

   273

   274

  if (unlikely(master->debug_level > 0)) {

   275

    printk(KERN_DEBUG "EtherCAT: Device send...\n");

   276

  }

   277

   278

  // Send frame

   279

  if (unlikely(ec_device_send(&master->device, master->tx_data,

   280

                              framelength) != 0)) {

   281

    printk(KERN_ERR "EtherCAT: Could not send!\n");

   282

    return -1;

   283

  }

   284

   285

  if (unlikely(master->debug_level > 0)) {

   286

    printk(KERN_DEBUG "EtherCAT: ec_simple_send done.\n");

   287

  }

   288

   289

  return 0;

   290

}

   291

   292

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

   293

   294

/**

   295

   Wartet auf den Empfang eines einzeln gesendeten

   296

   Kommandos.

   297

   298

   @param master EtherCAT-Master

   299

   @param cmd    Gesendetes Kommando

   300

   301

   @return 0 bei Erfolg, sonst < 0

   302

*/

   303

   304

int ec_simple_receive(ec_master_t *master, ec_command_t *cmd)

   305

{

   306

  unsigned int length;

   307

  int ret;

   308

  unsigned char command_type, command_index;

   309

   310

  if (unlikely((ret = ec_device_receive(&master->device, master->rx_data)) < 0))

   311

    return -1;

   312

   313

  master->rx_data_length = (unsigned int) ret;

   314

   315

  if (unlikely(master->rx_data_length < 2)) {

   316

    printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT"

   317

           " header!\n");

   318

    ec_output_debug_data(master);

   319

    return -1;

   320

  }

   321

   322

  // Länge des gesamten Frames prüfen

   323

  length = ((master->rx_data[1] & 0x07) << 8)

   324

    | (master->rx_data[0] & 0xFF);

   325

   326

  if (unlikely(length > master->rx_data_length)) {

   327

    printk(KERN_ERR "EtherCAT: Received corrupted frame (length does"

   328

           " not match)!\n");

   329

    ec_output_debug_data(master);

   330

    return -1;

   331

  }

   332

   333

  command_type = master->rx_data[2];

   334

  command_index = master->rx_data[2 + 1];

   335

  length = (master->rx_data[2 + 6] & 0xFF)

   336

    | ((master->rx_data[2 + 7] & 0x07) << 8);

   337

   338

  if (unlikely(master->rx_data_length - 2 < length + 12)) {

   339

    printk(KERN_ERR "EtherCAT: Received frame with"

   340

           " incomplete command data!\n");

   341

    ec_output_debug_data(master);

   342

    return -1;

   343

  }

   344

   345

  if (likely(cmd->state == EC_COMMAND_STATE_SENT

   346

             && cmd->type == command_type

   347

             && cmd->index == command_index

   348

             && cmd->data_length == length))

   349

  {

   350

    cmd->state = EC_COMMAND_STATE_RECEIVED;

   351

   352

    // Empfangene Daten in Kommandodatenspeicher kopieren

   353

    memcpy(cmd->data, master->rx_data + 2 + 10, length);

   354

   355

    // Working-Counter setzen

   356

    cmd->working_counter

   357

      = ((master->rx_data[length + 2 + 10] & 0xFF)

   358

         | ((master->rx_data[length + 2 + 11] & 0xFF) << 8));

   359

   360

    if (unlikely(master->debug_level > 1)) {

   361

      ec_output_debug_data(master);

   362

    }

   363

  }

   364

  else

   365

  {

   366

    printk(KERN_WARNING "EtherCAT: WARNING - Send/Receive anomaly!\n");

   367

    ec_output_debug_data(master);

   368

  }

   369

   370

  master->device.state = EC_DEVICE_STATE_READY;

   371

   372

  return 0;

   373

}

   374

   375

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

   376

   377

/**

   378

   Durchsucht den Bus nach Slaves.

   379

   380

   @param master Der EtherCAT-Master

   381

   382

   @return 0 bei Erfolg, sonst < 0

   383

*/

   384

   385

int ec_scan_for_slaves(ec_master_t *master)

   386

{

   387

  ec_command_t cmd;

   388

  ec_slave_t *slave;

   389

  unsigned int i, j;

   390

  unsigned char data[2];

   391

   392

  // Determine number of slaves on bus

   393

   394

  ec_command_broadcast_read(&cmd, 0x0000, 4);

   395

   396

  if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   397

    return -1;

   398

   399

  master->bus_slaves_count = cmd.working_counter;

   400

  printk("EtherCAT: Found %i slaves on bus.\n", master->bus_slaves_count);

   401

   402

  if (!master->bus_slaves_count) return 0;

   403

   404

  if (!(master->bus_slaves = (ec_slave_t *) kmalloc(master->bus_slaves_count

   405

                                                    * sizeof(ec_slave_t),

   406

                                                    GFP_KERNEL))) {

   407

    printk(KERN_ERR "EtherCAT: Could not allocate memory for bus slaves!\n");

   408

    return -1;

   409

  }

   410

   411

  // For every slave in the list

   412

  for (i = 0; i < master->bus_slaves_count; i++)

   413

  {

   414

    slave = master->bus_slaves + i;

   415

   416

    ec_slave_init(slave);

   417

   418

    // Set ring position

   419

    slave->ring_position = -i;

   420

    slave->station_address = i + 1;

   421

   422

    // Write station address

   423

   424

    data[0] = slave->station_address & 0x00FF;

   425

    data[1] = (slave->station_address & 0xFF00) >> 8;

   426

   427

    ec_command_position_write(&cmd, slave->ring_position, 0x0010, 2, data);

   428

   429

    if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   430

      return -1;

   431

   432

    if (unlikely(cmd.working_counter != 1)) {

   433

      printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing"

   434

             " station address!\n", i);

   435

      return -1;

   436

    }

   437

   438

    // Read base data

   439

   440

    ec_command_read(&cmd, slave->station_address, 0x0000, 4);

   441

   442

    if (unlikely(ec_simple_send_receive(master, &cmd) < 0))

   443

      return -1;

   444

   445

    if (unlikely(cmd.working_counter != 1)) {

   446

      printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base"

   447

             " data!\n", i);

   448

      return -1;

   449

    }

   450

   451

    // Get base data

   452

   453

    slave->base_type = cmd.data[0];

   454

    slave->base_revision = cmd.data[1];

   455

    slave->base_build = cmd.data[2] | (cmd.data[3] << 8);

   456

   457

    // Read identification from "Slave Information Interface" (SII)

   458

   459

    if (unlikely(ec_sii_read(master, slave->station_address, 0x0008,

   460

                             &slave->sii_vendor_id) != 0)) {

   461

      printk(KERN_ERR "EtherCAT: Could not read SII vendor id!\n");

   462

      return -1;

   463

    }

   464

   465

    if (unlikely(ec_sii_read(master, slave->station_address, 0x000A,

   466

                             &slave->sii_product_code) != 0)) {

   467

      printk(KERN_ERR "EtherCAT: Could not read SII product code!\n");

   468

      return -1;

   469

    }

   470

   471

    if (unlikely(ec_sii_read(master, slave->station_address, 0x000C,

   472

                             &slave->sii_revision_number) != 0)) {

   473

      printk(KERN_ERR "EtherCAT: Could not read SII revision number!\n");

   474

      return -1;

   475

    }

   476

   477

    if (unlikely(ec_sii_read(master, slave->station_address, 0x000E,

   478

                             &slave->sii_serial_number) != 0)) {

   479

      printk(KERN_ERR "EtherCAT: Could not read SII serial number!\n");

   480

      return -1;

   481

    }

   482

   483

    // Search for identification in "database"

   484

   485

    for (j = 0; j < slave_ident_count; j++)

   486

    {

   487

      if (unlikely(slave_idents[j].vendor_id == slave->sii_vendor_id

   488

                   && slave_idents[j].product_code == slave->sii_product_code))