1

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

     2

 *

     3

 *  e c _ m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Date$

     8

 *  $Author$

     9

 *

    10

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

    11

    12

#include <linux/module.h>

    13

#include <linux/kernel.h>

    14

#include <linux/string.h>

    15

#include <linux/slab.h>

    16

#include <linux/delay.h>

    17

    18

#include "ec_globals.h"

    19

#include "ec_master.h"

    20

    21

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

    22

    23

/**

    24

   Konstruktor des EtherCAT-Masters.

    25

    26

   @param master Zeiger auf den zu initialisierenden

    27

                 EtherCAT-Master

    28

    29

   @return 0 bei Erfolg, sonst < 0 (= dev ist NULL)

    30

*/

    31

    32

void EtherCAT_master_init(EtherCAT_master_t *master)

    33

{

    34

  master->slaves = NULL;

    35

  master->slave_count = 0;

    36

  master->dev = NULL;

    37

  master->command_index = 0x00;

    38

  master->tx_data_length = 0;

    39

  master->process_data = NULL;

    40

  master->process_data_length = 0;

    41

  master->debug_level = 0;

    42

}

    43

    44

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

    45

    46

/**

    47

   Destruktor eines EtherCAT-Masters.

    48

    49

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

    50

   auf das Slave-Array und gibt die Prozessdaten frei.

    51

    52

   @param master Zeiger auf den zu löschenden Master

    53

*/

    54

    55

void EtherCAT_master_clear(EtherCAT_master_t *master)

    56

{

    57

  // Remove all slaves

    58

  EtherCAT_clear_slaves(master);

    59

    60

  if (master->process_data)

    61

  {

    62

    kfree(master->process_data);

    63

    master->process_data = NULL;

    64

  }

    65

    66

  master->process_data_length = 0;

    67

}

    68

    69

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

    70

    71

/**

    72

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

    73

    74

   Registriert das Geraet beim master, der es daraufhin oeffnet.

    75

    76

   @param master Der EtherCAT-Master

    77

   @param device Das EtherCAT-Geraet

    78

   @return 0, wenn alles o.k.,

    79

           < 0, wenn bereits ein Geraet registriert

    80

           oder das Geraet nicht geoeffnet werden konnte.

    81

*/

    82

    83

int EtherCAT_register_device(EtherCAT_master_t *master,

    84

                             EtherCAT_device_t *device)

    85

{

    86

  if (!master || !device)

    87

  {

    88

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

    89

    return -1;

    90

  }

    91

    92

  if (master->dev)

    93

  {

    94

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

    95

    return -1;

    96

  }

    97

    98

  if (EtherCAT_device_open(device) < 0)

    99

  {

   100

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

   101

           (unsigned int) master->dev);

   102

    return -1;

   103

  }

   104

   105

  master->dev = device;

   106

   107

  return 0;

   108

}

   109

   110

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

   111

   112

/**

   113

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

   114

   115

   @param master Der EtherCAT-Master

   116

   @param device Das EtherCAT-Geraet

   117

*/

   118

   119

void EtherCAT_unregister_device(EtherCAT_master_t *master,

   120

                                EtherCAT_device_t *device)

   121

{

   122

  if (master->dev != device)

   123

  {

   124

    printk(KERN_WARNING "EtherCAT: Trying to unregister unknown device.\n");

   125

    return;

   126

  }

   127

   128

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

   129

  {

   130

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

   131

  }

   132

   133

  master->dev = NULL;

   134

}

   135

   136

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

   137

   138

/**

   139

   Sendet ein einzelnes Kommando in einem Frame und

   140

   wartet auf dessen Empfang.

   141

   142

   @param master EtherCAT-Master

   143

   @param cmd    Kommando zum Senden/Empfangen

   144

   145

   @return 0 bei Erfolg, sonst < 0

   146

*/

   147

   148

int EtherCAT_simple_send_receive(EtherCAT_master_t *master,

   149

                                 EtherCAT_command_t *cmd)

   150

{

   151

  unsigned int tries_left;

   152

   153

  if (EtherCAT_simple_send(master, cmd) < 0) return -1;

   154

   155

  udelay(3);

   156

   157

  EtherCAT_device_call_isr(master->dev);

   158

   159

  tries_left = 20;

   160

  while (master->dev->state == ECAT_DS_SENT && tries_left)

   161

  {

   162

    udelay(1);

   163

    EtherCAT_device_call_isr(master->dev);

   164

    tries_left--;

   165

  }

   166

   167

  if (EtherCAT_simple_receive(master, cmd) < 0) return -1;

   168

   169

  return 0;

   170

}

   171

   172

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

   173

   174

/**

   175

   Sendet ein einzelnes Kommando in einem Frame.

   176

   177

   @param master EtherCAT-Master

   178

   @param cmd    Kommando zum Senden

   179

   180

   @return 0 bei Erfolg, sonst < 0

   181

*/

   182

   183

int EtherCAT_simple_send(EtherCAT_master_t *master,

   184

                         EtherCAT_command_t *cmd)

   185

{

   186

  unsigned int length, framelength, i;

   187

   188

  if (master->debug_level > 0)

   189

  {

   190

    printk(KERN_DEBUG "EtherCAT_send_receive_command\n");

   191

  }

   192

   193

  if (cmd->state != ECAT_CS_READY)

   194

  {

   195

    printk(KERN_WARNING "EtherCAT_send_receive_command: Command not in ready state!\n");

   196

  }

   197

   198

  length = cmd->data_length + 12;

   199

  framelength = length + 2;

   200

   201

  if (framelength > ECAT_FRAME_BUFFER_SIZE)

   202

  {

   203

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

   204

    return -1;

   205

  }

   206

   207

  if (framelength < 46) framelength = 46;

   208

   209

  if (master->debug_level > 0)

   210

  {

   211

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

   212

  }

   213

   214

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

   215

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

   216

   217

  cmd->index = master->command_index;

   218

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

   219

   220

  if (master->debug_level > 0)

   221

  {

   222

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

   223

  }

   224

   225

  cmd->state = ECAT_CS_SENT;

   226

   227

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

   228

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

   229

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

   230

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

   231

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

   232

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

   233

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

   234

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

   235

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

   236

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

   237

   238

  if (cmd->type == ECAT_CMD_APWR

   239

      || cmd->type == ECAT_CMD_NPWR

   240

      || cmd->type == ECAT_CMD_BWR

   241

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

   242

  {

   243

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

   244

  }

   245

  else // Read

   246

  {

   247

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

   248

  }

   249

   250

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

   251

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

   252

   253

  // Pad with zeros

   254

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

   255

   256

  master->tx_data_length = framelength;

   257

   258

  if (master->debug_level > 0)

   259

  {

   260

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

   261

  }

   262

   263

  // Send frame

   264

  if (EtherCAT_device_send(master->dev, master->tx_data, framelength) != 0)

   265

  {

   266

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

   267

    return -1;

   268

  }

   269

   270

  if (master->debug_level > 0)

   271

  {

   272

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

   273

  }

   274

   275

  return 0;

   276

}

   277

   278

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

   279

   280

/**

   281

   Wartet auf den Empfang eines einzeln gesendeten

   282

   Kommandos.

   283

   284

   @param master EtherCAT-Master

   285

   @param cmd    Gesendetes Kommando

   286

   287

   @return 0 bei Erfolg, sonst < 0

   288

*/

   289

   290

int EtherCAT_simple_receive(EtherCAT_master_t *master,

   291

                            EtherCAT_command_t *cmd)

   292

{

   293

  unsigned int length;

   294

  int receive_ret;

   295

  unsigned char command_type, command_index;

   296

   297

  if ((receive_ret = EtherCAT_device_receive(master->dev,

   298

                                             master->rx_data)) < 0)

   299

  {

   300

    return -1;

   301

  }

   302

   303

  master->rx_data_length = (unsigned int) receive_ret;

   304

   305

  if (master->rx_data_length < 2)

   306

  {

   307

    printk(KERN_ERR "EtherCAT: Received frame with incomplete EtherCAT header!\n");

   308

    output_debug_data(master);

   309

    return -1;

   310

  }

   311

   312

  // Länge des gesamten Frames prüfen

   313

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

   314

   315

  if (length > master->rx_data_length)

   316

  {

   317

    printk(KERN_ERR "EtherCAT: Received corrupted frame (length does not match)!\n");

   318

    output_debug_data(master);

   319

    return -1;

   320

  }

   321

   322

  command_type = master->rx_data[2];

   323

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

   324

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

   325

   326

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

   327

  {

   328

    printk(KERN_ERR "EtherCAT: Received frame with incomplete command data!\n");

   329

    output_debug_data(master);

   330

    return -1;

   331

  }

   332

   333

  if (cmd->state == ECAT_CS_SENT

   334

      && cmd->type == command_type

   335

      && cmd->index == command_index

   336

      && cmd->data_length == length)

   337

  {

   338

    cmd->state = ECAT_CS_RECEIVED;

   339

   340

    // Empfangene Daten in Kommandodatenspeicher kopieren

   341

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

   342

   343

    // Working-Counter setzen

   344

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

   345

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

   346

  }

   347

  else

   348

  {

   349

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

   350

    output_debug_data(master);

   351

  }

   352

   353

  master->dev->state = ECAT_DS_READY;

   354

   355

  return 0;

   356

}

   357

   358

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

   359

   360

/**

   361

   Überprüft die angeschlossenen Slaves.

   362

   363

   Vergleicht die an den Bus angeschlossenen Slaves mit

   364

   den im statischen-Slave-Array vorgegebenen Konfigurationen.

   365

   Stimmen Anzahl oder Typen nicht überein, gibt diese

   366

   Methode einen Fehler aus.

   367

   368

   @param master Der EtherCAT-Master

   369

   @param slaves Zeiger auf ein statisches Slave-Array

   370

   @param slave_count Anzahl der Slaves im Array

   371

   372

   @return 0 bei Erfolg, sonst < 0

   373

*/

   374

   375

int EtherCAT_check_slaves(EtherCAT_master_t *master,

   376

                          EtherCAT_slave_t *slaves,

   377

                          unsigned int slave_count)

   378

{

   379

  EtherCAT_command_t cmd;

   380

  EtherCAT_slave_t *cur;

   381

  unsigned int i, j, found, length, pos;

   382

  unsigned char data[2];

   383

   384

  if (slave_count == 0)

   385

  {

   386

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

   387

    return -1;

   388

  }

   389

   390

  // Determine number of slaves on bus

   391

   392

  EtherCAT_command_broadcast_read(&cmd, 0x0000, 4);

   393

   394

  if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   395

   396

  if (cmd.working_counter != slave_count)

   397

  {

   398

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

   399

           cmd.working_counter, slave_count);

   400

    return -1;

   401

  }

   402

  else

   403

  {

   404

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

   405

  }

   406

   407

  // For every slave in the list

   408

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

   409

  {

   410

    cur = &slaves[i];

   411

   412

    if (!cur->desc)

   413

    {

   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, 0x0010, 2, data);

   428

    if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   429

   430

    if (cmd.working_counter != 1)

   431

    {

   432

      printk(KERN_ERR "EtherCAT: Slave %i did not repond while writing station address!\n", i);

   433

      return -1;

   434

    }

   435

   436

    // Read base data

   437

   438

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

   439

    if (EtherCAT_simple_send_receive(master, &cmd) < 0) return -1;

   440

   441

    if (cmd.working_counter != 1)

   442

    {

   443

      printk(KERN_ERR "EtherCAT: Slave %i did not respond while reading base data!\n", i);

   444

      return -1;

   445

    }

   446

   447

    // Get base data

   448

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

   449

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

   450

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

   451

   452

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

   453

   454

    if (EtherCAT_read_slave_information(master, cur->station_address,

   455

                                        0x0008, &cur->vendor_id) != 0)

   456

    {

   457

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

   458

      return -1;

   459

    }

   460

   461

    if (EtherCAT_read_slave_information(master, cur->station_address,

   462

                                        0x000A, &cur->product_code) != 0)

   463

    {

   464

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

   465

      return -1;

   466

    }

   467

   468

    if (EtherCAT_read_slave_information(master, cur->station_address,

   469

                                        0x000C, &cur->revision_number) != 0)

   470

    {

   471

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

   472

      return -1;

   473

    }

   474

   475

    if (EtherCAT_read_slave_information(master, cur->station_address,

   476

                                        0x000E, &cur->serial_number) != 0)

   477

    {

   478

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

   479

      return -1;

   480

    }

   481

   482

    // Search for identification in "database"

   483

   484

    found = 0;

   485

   486

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

   487

    {

   488

      if (slave_idents[j].vendor_id == cur->vendor_id

   489

          && slave_idents[j].product_code == cur->product_code)

   490

      {

   491

        found = 1;

   492

   493

        if (cur->desc != slave_idents[j].desc)

   494

        {

   495

          printk(KERN_ERR "EtherCAT: Unexpected slave device \"%s %s\""

   496