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

    41

void ec_master_init(ec_master_t *master

    42

                    /**< Zeiger auf den zu initialisierenden EtherCAT-Master */

    43

                    )

    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

    67

void ec_master_clear(ec_master_t *master

    68

                     /**< Zeiger auf den zu löschenden Master */

    69

                     )

    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

/**

    84

   Setzt den Master in den Ausgangszustand.

    85

    86

   Bei einem "release" sollte immer diese Funktion aufgerufen werden,

    87

   da sonst Slave-Liste, Domains, etc. weiter existieren.

    88

*/

    89

    90

void ec_master_reset(ec_master_t *master

    91

                     /**< Zeiger auf den zurückzusetzenden Master */

    92

                     )

    93

{

    94

  if (master->bus_slaves) {

    95

    kfree(master->bus_slaves);

    96

    master->bus_slaves = NULL;

    97

  }

    98

    99

  master->bus_slaves_count = 0;

   100

  master->command_index = 0;

   101

  master->tx_data_length = 0;

   102

  master->rx_data_length = 0;

   103

  master->domain_count = 0;

   104

  master->debug_level = 0;

   105

  master->bus_time = 0;

   106

  master->frames_lost = 0;

   107

  master->t_lost_output = 0;

   108

}

   109

   110

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

   111

   112

/**

   113

   Öffnet das EtherCAT-Geraet des Masters.

   114

   115

   \return 0, wenn alles o.k., < 0, wenn kein Gerät registriert wurde oder

   116

   es nicht geoeffnet werden konnte.

   117

*/

   118

   119

int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)

   120

{

   121

  if (!master->device_registered) {

   122

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

   123

    return -1;

   124

  }

   125

   126

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

   127

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

   128

    return -1;

   129

  }

   130

   131

  return 0;

   132

}

   133

   134

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

   135

   136

/**

   137

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

   138

*/

   139

   140

void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)

   141

{

   142

  if (!master->device_registered) {

   143

    printk(KERN_WARNING "EtherCAT: Warning -"

   144

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

   145

    return;

   146

  }

   147

   148

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

   149

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

   150

  }

   151

}

   152

   153

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

   154

   155

/**

   156

   Sendet ein einzelnes Kommando in einem Frame und

   157

   wartet auf dessen Empfang.

   158

   159

   \return 0 bei Erfolg, sonst < 0

   160

*/

   161

   162

int ec_simple_send_receive(ec_master_t *master,

   163

                           /**< EtherCAT-Master */

   164

                           ec_command_t *cmd

   165

                           /**< Kommando zum Senden/Empfangen */

   166

                           )

   167

{

   168

  unsigned int tries_left;

   169

   170

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

   171

    return -1;

   172

   173

  tries_left = 20;

   174

   175

  do

   176

  {

   177

    udelay(1);

   178

    ec_device_call_isr(&master->device);

   179

    tries_left--;

   180

  }

   181

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

   182

   183

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

   184

    return -1;

   185

   186

  return 0;

   187

}

   188

   189

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

   190

   191

/**

   192

   Sendet ein einzelnes Kommando in einem Frame.

   193

   194

   \return 0 bei Erfolg, sonst < 0

   195

*/

   196

   197

int ec_simple_send(ec_master_t *master, /**< EtherCAT-Master */

   198

                   ec_command_t *cmd /**< Kommando zum Senden */

   199

                   )

   200

{

   201

  unsigned int length, framelength, i;

   202

   203

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

   204

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

   205

  }

   206

   207

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

   208

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

   209

  }

   210

   211

  length = cmd->data_length + 12;

   212

  framelength = length + 2;

   213

   214

  if (unlikely(framelength > EC_FRAME_SIZE)) {

   215

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

   216

    return -1;

   217

  }

   218

   219

  if (framelength < 46) framelength = 46;

   220

   221

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

   222

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

   223

  }

   224

   225

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

   226

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

   227

   228

  cmd->index = master->command_index;

   229

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

   230

   231

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

   232

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

   233

  }

   234

   235

  cmd->state = EC_COMMAND_STATE_SENT;

   236

   237

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

   238

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

   239

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

   240

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

   241

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

   242

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

   243

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

   244

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

   245

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

   246

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

   247

   248

  if (likely(cmd->type == EC_COMMAND_APWR

   249

             || cmd->type == EC_COMMAND_NPWR

   250

             || cmd->type == EC_COMMAND_BWR

   251

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

   252

  {

   253

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

   254

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

   255

  }

   256

  else // Read commands

   257

  {

   258

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

   259

  }

   260

   261

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

   262

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

   263

   264

  // Pad with zeros

   265

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

   266

   267

  master->tx_data_length = framelength;

   268

   269

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

   270

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

   271

  }

   272

   273

  // Send frame

   274

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

   275

                              framelength) != 0)) {

   276

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

   277

    return -1;

   278

  }

   279

   280

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

   281

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

   282

  }

   283

   284

  return 0;

   285

}

   286

   287

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

   288

   289

/**

   290

   Wartet auf den Empfang eines einzeln gesendeten

   291

   Kommandos.

   292

   293

   \return 0 bei Erfolg, sonst < 0

   294

*/

   295

   296

int ec_simple_receive(ec_master_t *master, /**< EtherCAT-Master */

   297

                      ec_command_t *cmd /**< Gesendetes Kommando */

   298

                      )

   299

{

   300

  unsigned int length;

   301

  int ret;

   302

  unsigned char command_type, command_index;

   303

   304

  if (unlikely((ret = ec_device_receive(&master->device,

   305

                                        master->rx_data)) < 0))

   306

    return -1;

   307

   308

  master->rx_data_length = (unsigned int) ret;

   309

   310

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

   311

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

   312

           " header!\n");

   313

    ec_output_debug_data(master);

   314

    return -1;

   315

  }

   316

   317

  // Länge des gesamten Frames prüfen

   318

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

   319

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

   320

   321

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

   322

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

   323

           " not match)!\n");

   324

    ec_output_debug_data(master);

   325

    return -1;

   326

  }

   327

   328

  command_type = master->rx_data[2];

   329

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

   330

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

   331

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

   332

   333

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

   334

    printk(KERN_ERR "EtherCAT: Received frame with"

   335

           " incomplete command data!\n");

   336

    ec_output_debug_data(master);

   337

    return -1;

   338

  }

   339

   340

  if (likely(cmd->state == EC_COMMAND_STATE_SENT

   341

             && cmd->type == command_type

   342

             && cmd->index == command_index

   343

             && cmd->data_length == length))

   344

  {

   345

    cmd->state = EC_COMMAND_STATE_RECEIVED;

   346

   347

    // Empfangene Daten in Kommandodatenspeicher kopieren

   348

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

   349

   350

    // Working-Counter setzen

   351

    cmd->working_counter

   352

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

   353

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

   354

   355

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

   356

      ec_output_debug_data(master);

   357

    }

   358

  }

   359

  else

   360

  {

   361

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

   362

    ec_output_debug_data(master);

   363

  }

   364

   365

  master->device.state = EC_DEVICE_STATE_READY;

   366

   367

  return 0;

   368

}

   369

   370

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

   371

   372

/**

   373

   Durchsucht den Bus nach Slaves.

   374

   375

   @return 0 bei Erfolg, sonst < 0

   376

*/

   377

   378

int ec_scan_for_slaves(ec_master_t *master /**< EtherCAT-Master */)

   379

{

   380

  ec_command_t cmd;

   381

  ec_slave_t *slave;

   382

  unsigned int i, j;

   383

  unsigned char data[2];

   384

   385

  // Determine number of slaves on bus

   386

   387

  ec_command_broadcast_read(&cmd, 0x0000, 4);

   388

   389

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

   390

    return -1;

   391

   392

  master->bus_slaves_count = cmd.working_counter;

   393

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

   394

   395

  if (!master->bus_slaves_count) return 0;

   396

   397

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

   398

                                                    * sizeof(ec_slave_t),

   399

                                                    GFP_KERNEL))) {

   400

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

   401

    return -1;

   402

  }

   403

   404

  // For every slave in the list

   405

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

   406

  {

   407

    slave = master->bus_slaves + i;

   408

   409

    ec_slave_init(slave);

   410

   411

    // Set ring position

   412

   413

    slave->ring_position = -i;

   414

    slave->station_address = i + 1;

   415

   416

    // Write station address

   417

   418

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

   419

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

   420

   421

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

   422

   423

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

   424

      return -1;

   425

   426

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

   427

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

   428

             " station address!\n", i);

   429

      return -1;

   430

    }

   431

   432

    // Read base data

   433

   434

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

   435

   436

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

   437

      return -1;

   438

   439

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

   440

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

   441

             " data!\n", i);

   442

      return -1;

   443

    }

   444

   445

    // Get base data

   446

   447

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

   448

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

   449

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

   450

   451

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

   452

   453

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

   454

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

   455

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

   456

      return -1;

   457

    }

   458

   459

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

   460

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

   461

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

   462

      return -1;

   463

    }

   464

   465

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

   466

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

   467

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

   468

      return -1;

   469

    }

   470

   471

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

   472

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

   473

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

   474

      return -1;

   475

    }

   476

   477

    // Search for identification in "database"

   478

   479

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

   480

    {

   481

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

   482

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

   483

      {

   484

        slave->type = slave_idents[j].type;

   485

        break;

   486

      }

   487

    }