1

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

     2

 *

     3

 *  e c _ 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 "ec_globals.h"

    18

#include "ec_master.h"

    19

    20

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

    21

    22

/**

    23

   Konstruktor des EtherCAT-Masters.

    24

    25

   @param master Zeiger auf den zu initialisierenden EtherCAT-Master

    26

*/

    27

    28

void EtherCAT_master_init(EtherCAT_master_t *master)

    29

{

    30

  master->dev = NULL;

    31

  master->command_index = 0x00;

    32

  master->tx_data_length = 0;

    33

  master->rx_data_length = 0;

    34

  master->domain_count = 0;

    35

  master->debug_level = 0;

    36

  master->bus_time = 0;

    37

  master->frames_lost = 0;

    38

  master->t_lost_output = 0;

    39

}

    40

    41

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

    42

    43

/**

    44

   Destruktor eines EtherCAT-Masters.

    45

    46

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

    47

   auf das Slave-Array und gibt die Prozessdaten frei.

    48

    49

   @param master Zeiger auf den zu löschenden Master

    50

*/

    51

    52

void EtherCAT_master_clear(EtherCAT_master_t *master)

    53

{

    54

  unsigned int i;

    55

    56

  // Remove domains

    57

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

    58

    EtherCAT_domain_clear(master->domains + i);

    59

  }

    60

    61

  master->domain_count = 0;

    62

}

    63

    64

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

    65

    66

/**

    67

   Öffnet ein EtherCAT-Geraet für den Master.

    68

    69

   Registriert das Geraet beim Master, der es daraufhin oeffnet.

    70

    71

   @param master Der EtherCAT-Master

    72

   @param device Das EtherCAT-Geraet

    73

   @return 0, wenn alles o.k.,

    74

   < 0, wenn bereits ein Geraet registriert

    75

   oder das Geraet nicht geoeffnet werden konnte.

    76

*/

    77

    78

int EtherCAT_master_open(EtherCAT_master_t *master,

    79

                         EtherCAT_device_t *device)

    80

{

    81

  if (!master || !device) {

    82

    printk(KERN_ERR "EtherCAT: Illegal parameters for master_open()!\n");

    83

    return -1;

    84

  }

    85

    86

  if (master->dev) {

    87

    printk(KERN_ERR "EtherCAT: Master already has a device.\n");

    88

    return -1;

    89

  }

    90

    91

  if (EtherCAT_device_open(device) < 0) {

    92

    printk(KERN_ERR "EtherCAT: Could not open device %X!\n",

    93

           (unsigned int) master->dev);

    94

    return -1;

    95

  }

    96

    97

  master->dev = device;

    98

    99

  return 0;

   100

}

   101

   102

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

   103

   104

/**

   105

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

   106

   107

   @param master Der EtherCAT-Master

   108

   @param device Das EtherCAT-Geraet

   109

*/

   110

   111

void EtherCAT_master_close(EtherCAT_master_t *master,

   112

                           EtherCAT_device_t *device)

   113

{

   114

  if (master->dev != device) {

   115

    printk(KERN_WARNING "EtherCAT: Warning -"

   116

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

   117

    return;

   118

  }

   119

   120

  if (EtherCAT_device_close(master->dev) < 0) {

   121

    printk(KERN_WARNING "EtherCAT: Warning -"

   122

           " Could not close device!\n");

   123

  }

   124

   125

  master->dev = NULL;

   126

}

   127

   128

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

   129

   130

/**

   131

   Sendet ein einzelnes Kommando in einem Frame und

   132

   wartet auf dessen Empfang.

   133

   134

   @param master EtherCAT-Master

   135

   @param cmd    Kommando zum Senden/Empfangen

   136

   137

   @return 0 bei Erfolg, sonst < 0

   138

*/

   139

   140

int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

   141

                                 EtherCAT_command_t *cmd)

   142

{

   143

  unsigned int tries_left;

   144

   145

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

   146

    return -1;

   147

   148

  udelay(3);

   149

   150

  EtherCAT_device_call_isr(master->dev);

   151

   152

  tries_left = 20;

   153

  while (unlikely(master->dev->state == ECAT_DS_SENT

   154

                  && tries_left)) {

   155

    udelay(1);

   156

    EtherCAT_device_call_isr(master->dev);

   157

    tries_left--;

   158

  }

   159

   160

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

   161

    return -1;

   162

   163

  return 0;

   164

}

   165

   166

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

   167

   168

/**

   169

   Sendet ein einzelnes Kommando in einem Frame.

   170

   171

   @param master EtherCAT-Master

   172

   @param cmd    Kommando zum Senden

   173

   174

   @return 0 bei Erfolg, sonst < 0

   175

*/

   176

   177

int EtherCAT_simple_send(EtherCAT_master_t *master,

   178

                         EtherCAT_command_t *cmd)

   179

{

   180

  unsigned int length, framelength, i;

   181

   182

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

   183

    printk(KERN_DEBUG "EtherCAT_send_receive_command\n");

   184

  }

   185

   186

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

   187

    printk(KERN_WARNING "EtherCAT_send_receive_command:"

   188

           "Command not in ready state!\n");

   189

  }

   190

   191

  length = cmd->data_length + 12;

   192

  framelength = length + 2;

   193

   194

  if (unlikely(framelength > ECAT_FRAME_BUFFER_SIZE)) {

   195

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

   196

    return -1;

   197

  }

   198

   199

  if (framelength < 46) framelength = 46;

   200

   201

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

   202

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

   203

  }

   204

   205

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

   206

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

   207

   208

  cmd->index = master->command_index;

   209

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

   210

   211

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

   212

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

   213

  }

   214

   215

  cmd->state = ECAT_CS_SENT;

   216

   217

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

   218

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

   219

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

   220

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

   221

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

   222

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

   223

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

   224

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

   225

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

   226

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

   227

   228

  if (likely(cmd->type == ECAT_CMD_APWR

   229

             || cmd->type == ECAT_CMD_NPWR

   230

             || cmd->type == ECAT_CMD_BWR

   231

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

   232

  {

   233

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

   234

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

   235

  }

   236

  else // Read commands

   237

  {

   238

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

   239

  }

   240

   241

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

   242

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

   243

   244

  // Pad with zeros

   245

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

   246

   247

  master->tx_data_length = framelength;

   248

   249

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

   250

    printk(KERN_DEBUG "device send...\n");

   251

  }

   252

   253

  // Send frame

   254

  if (unlikely(EtherCAT_device_send(master->dev,

   255

                                    master->tx_data,

   256

                                    framelength) != 0)) {

   257

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

   258

    return -1;

   259

  }

   260

   261

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

   262

    printk(KERN_DEBUG "EtherCAT_send done.\n");

   263

  }

   264

   265

  return 0;

   266

}

   267

   268

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

   269

   270

/**

   271

   Wartet auf den Empfang eines einzeln gesendeten

   272

   Kommandos.

   273

   274

   @param master EtherCAT-Master

   275

   @param cmd    Gesendetes Kommando

   276

   277

   @return 0 bei Erfolg, sonst < 0

   278

*/

   279

   280

int EtherCAT_simple_receive(EtherCAT_master_t *master,

   281

                            EtherCAT_command_t *cmd)

   282

{

   283

  unsigned int length;

   284

  int ret;

   285

  unsigned char command_type, command_index;

   286

   287

  if (unlikely((ret = EtherCAT_device_receive(master->dev,

   288

                                              master->rx_data)) < 0))

   289

    return -1;

   290

   291

  master->rx_data_length = (unsigned int) ret;

   292

   293

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

   294

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

   295

           " header!\n");

   296

    output_debug_data(master);

   297

    return -1;

   298

  }

   299

   300

  // Länge des gesamten Frames prüfen

   301

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

   302

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

   303

   304

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

   305

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

   306

           " not match)!\n");

   307

    output_debug_data(master);

   308

    return -1;

   309

  }

   310

   311

  command_type = master->rx_data[2];

   312

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

   313

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

   314

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

   315

   316

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

   317

    printk(KERN_ERR "EtherCAT: Received frame with"

   318

           " incomplete command data!\n");

   319

    output_debug_data(master);

   320

    return -1;

   321

  }

   322

   323

  if (likely(cmd->state == ECAT_CS_SENT

   324

             && cmd->type == command_type

   325

             && cmd->index == command_index

   326

             && cmd->data_length == length))

   327

  {

   328

    cmd->state = ECAT_CS_RECEIVED;

   329

   330

    // Empfangene Daten in Kommandodatenspeicher kopieren

   331

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

   332

   333

    // Working-Counter setzen

   334

    cmd->working_counter

   335

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

   336

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

   337

   338

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

   339

      output_debug_data(master);

   340

    }

   341

  }

   342

  else

   343

  {

   344

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

   345

    output_debug_data(master);

   346

  }

   347

   348

  master->dev->state = ECAT_DS_READY;

   349

   350

  return 0;

   351

}

   352

   353

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

   354

   355

/**

   356

   Überprüft die angeschlossenen Slaves.

   357

   358

   Vergleicht die an den Bus angeschlossenen Slaves mit

   359

   den im statischen-Slave-Array vorgegebenen Konfigurationen.

   360

   Stimmen Anzahl oder Typen nicht überein, gibt diese

   361

   Methode einen Fehler aus.

   362

   363

   @param master Der EtherCAT-Master

   364

   @param slaves Zeiger auf ein statisches Slave-Array

   365

   @param slave_count Anzahl der Slaves im Array

   366

   367

   @return 0 bei Erfolg, sonst < 0

   368

*/

   369

   370

int EtherCAT_check_slaves(EtherCAT_master_t *master,

   371

                          EtherCAT_slave_t *slaves,

   372

                          unsigned int slave_count)

   373

{

   374

  EtherCAT_command_t cmd;

   375

  EtherCAT_slave_t *cur;

   376

  unsigned int i, j, found, size, offset;

   377

  unsigned char data[2];

   378

  EtherCAT_domain_t *dom;

   379

   380

  // Clear domains

   381

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

   382

    printk(KERN_DEBUG "EtherCAT: Clearing domain %i!\n",

   383

           master->domains[i].number);

   384

    EtherCAT_domain_clear(master->domains + i);

   385

  }

   386

  master->domain_count = 0;

   387

   388

  if (unlikely(!slave_count)) {

   389

    printk(KERN_ERR "EtherCAT: No slaves in list!\n");

   390

    return -1;

   391

  }

   392

   393

  // Determine number of slaves on bus

   394

   395

  EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   396

   397

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

   398

    return -1;

   399

   400

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

   401

    printk(KERN_ERR "EtherCAT: Wrong number of slaves on bus: %i / %i\n",

   402

           cmd.working_counter, slave_count);

   403

    return -1;

   404

  }

   405

   406

  printk("EtherCAT: Found all %i slaves.\n", slave_count);

   407

   408

  // For every slave in the list

   409

  for (i = 0; i < slave_count; i++)

   410

  {

   411

    cur = &slaves[i];

   412

   413

    if (unlikely(!cur->desc)) {

   414

      printk(KERN_ERR "EtherCAT: Slave %i has no description.\n", i);

   415

      return -1;

   416

    }

   417

   418

    // Set ring position

   419

    cur->ring_position = -i;

   420

    cur->station_address = i + 1;

   421

   422

    // Write station address

   423

   424

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

   425

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

   426

   427

    EtherCAT_command_position_write(&cmd, cur->ring_position,

   428

                                    0x0010, 2, data);

   429

   430

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

   431

      return -1;

   432

   433

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

   434

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

   435

             " while writing station address!\n", i);

   436

      return -1;

   437

    }

   438

   439

    // Read base data

   440

   441

    EtherCAT_command_read(&cmd, cur->station_address, 0x0000, 4);

   442

   443

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

   444

      return -1;

   445

   446

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

   447

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

   448

             " while reading base data!\n", i);

   449

      return -1;

   450

    }

   451

   452

    // Get base data

   453

   454

    cur->type = cmd.data[0];

   455

    cur->revision = cmd.data[1];

   456

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

   457

   458

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

   459

   460

    if (unlikely(EtherCAT_read_slave_information(master,

   461

                                                 cur->station_address,

   462

                                                 0x0008,

   463

                                                 &cur->vendor_id) != 0))

   464

    {

   465

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

   466

      return -1;

   467

    }

   468

   469

    if (unlikely(EtherCAT_read_slave_information(master,

   470

                                                 cur->station_address,

   471

                                                 0x000A,

   472

                                                 &cur->product_code) != 0))

   473

    {

   474

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

   475

      return -1;

   476

    }

   477

   478

    if (unlikely(EtherCAT_read_slave_information(master,

   479

                                                 cur->station_address,

   480

                                                 0x000C,

   481

                                                 &cur->revision_number) != 0))

   482

    {

   483

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

   484

      return -1;

   485

    }

   486

   487

    if (unlikely(EtherCAT_read_slave_information(master,

   488

                                                 cur->station_address,

   489

                                                 0x000E,

   490

                                                 &cur->serial_number) != 0))