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/ecrt.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

#include "ethernet.h"

    25

    26

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

    27

    28

/**

    29

   Konstruktor des EtherCAT-Masters.

    30

*/

    31

    32

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

    33

{

    34

    master->slaves = NULL;

    35

    master->device = NULL;

    36

    37

    INIT_LIST_HEAD(&master->command_queue);

    38

    INIT_LIST_HEAD(&master->domains);

    39

    INIT_LIST_HEAD(&master->eoe_slaves);

    40

    41

    ec_command_init(&master->simple_command);

    42

    ec_command_init(&master->watch_command);

    43

    44

    ec_master_reset(master);

    45

}

    46

    47

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

    48

    49

/**

    50

   Destruktor eines EtherCAT-Masters.

    51

    52

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

    53

   auf das Slave-Array und gibt die Prozessdaten frei.

    54

*/

    55

    56

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

    57

{

    58

    ec_master_reset(master);

    59

    60

    if (master->device) {

    61

        ec_device_clear(master->device);

    62

        kfree(master->device);

    63

    }

    64

    65

    ec_command_clear(&master->simple_command);

    66

    ec_command_clear(&master->watch_command);

    67

}

    68

    69

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

    70

    71

/**

    72

   Setzt den Master zurück in den Ausgangszustand.

    73

    74

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

    75

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

    76

*/

    77

    78

void ec_master_reset(ec_master_t *master

    79

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

    80

                     )

    81

{

    82

    unsigned int i;

    83

    ec_command_t *command, *next_c;

    84

    ec_domain_t *domain, *next_d;

    85

    ec_eoe_t *eoe, *next_eoe;

    86

    87

    // Alle Slaves entfernen

    88

    if (master->slaves) {

    89

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

    90

            ec_slave_clear(master->slaves + i);

    91

        }

    92

        kfree(master->slaves);

    93

        master->slaves = NULL;

    94

    }

    95

    master->slave_count = 0;

    96

    97

    // Kommando-Warteschlange leeren

    98

    list_for_each_entry_safe(command, next_c, &master->command_queue, queue) {

    99

        command->state = EC_CMD_ERROR;

   100

        list_del_init(&command->queue);

   101

    }

   102

   103

    // Domain-Liste leeren

   104

    list_for_each_entry_safe(domain, next_d, &master->domains, list) {

   105

        list_del(&domain->list);

   106

        ec_domain_clear(domain);

   107

        kfree(domain);

   108

    }

   109

   110

    // EOE-Liste leeren

   111

    list_for_each_entry_safe(eoe, next_eoe, &master->domains, list) {

   112

        list_del(&eoe->list);

   113

        ec_eoe_clear(eoe);

   114

        kfree(eoe);

   115

    }

   116

   117

    master->command_index = 0;

   118

    master->debug_level = 0;

   119

    master->timeout = 100; // us

   120

   121

    master->slaves_responding = 0;

   122

    master->slave_states = EC_SLAVE_STATE_UNKNOWN;

   123

   124

    master->stats.timeouts = 0;

   125

    master->stats.delayed = 0;

   126

    master->stats.corrupted = 0;

   127

    master->stats.unmatched = 0;

   128

    master->stats.t_last = 0;

   129

}

   130

   131

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

   132

   133

/**

   134

   Öffnet das EtherCAT-Geraet des Masters.

   135

   136

   \return 0 wenn alles ok, < 0 wenn kein Gerät registriert wurde oder

   137

           es nicht geoeffnet werden konnte.

   138

*/

   139

   140

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

   141

{

   142

    if (!master->device) {

   143

        EC_ERR("No device registered!\n");

   144

        return -1;

   145

    }

   146

   147

    if (ec_device_open(master->device)) {

   148

        EC_ERR("Could not open device!\n");

   149

        return -1;

   150

    }

   151

   152

    return 0;

   153

}

   154

   155

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

   156

   157

/**

   158

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

   159

*/

   160

   161

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

   162

{

   163

    if (!master->device) {

   164

        EC_WARN("Warning - Trying to close an unregistered device!\n");

   165

        return;

   166

    }

   167

   168

    if (ec_device_close(master->device))

   169

        EC_WARN("Warning - Could not close device!\n");

   170

}

   171

   172

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

   173

   174

/**

   175

   Stellt ein Kommando in die Warteschlange.

   176

*/

   177

   178

void ec_master_queue_command(ec_master_t *master, /**< EtherCAT-Master */

   179

                             ec_command_t *command /**< Kommando */

   180

                             )

   181

{

   182

    ec_command_t *queued_command;

   183

   184

    // Ist das Kommando schon in der Warteschlange?

   185

    list_for_each_entry(queued_command, &master->command_queue, queue) {

   186

        if (queued_command == command) {

   187

            command->state = EC_CMD_QUEUED;

   188

            if (unlikely(master->debug_level))

   189

                EC_WARN("command already queued.\n");

   190

            return;

   191

        }

   192

    }

   193

   194

    list_add_tail(&command->queue, &master->command_queue);

   195

    command->state = EC_CMD_QUEUED;

   196

}

   197

   198

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

   199

   200

/**

   201

   Sendet die Kommandos in der Warteschlange.

   202

   203

   \return 0 bei Erfolg, sonst < 0

   204

*/

   205

   206

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

   207

{

   208

    ec_command_t *command;

   209

    size_t command_size;

   210

    uint8_t *frame_data, *cur_data;

   211

    void *follows_word;

   212

    cycles_t start = 0, end;

   213

   214

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

   215

        EC_DBG("ec_master_send\n");

   216

        start = get_cycles();

   217

    }

   218

   219

    // Zeiger auf Socket-Buffer holen

   220

    frame_data = ec_device_tx_data(master->device);

   221

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   222

    follows_word = NULL;

   223

   224

    // Aktuellen Frame mit Kommandos füllen

   225

    list_for_each_entry(command, &master->command_queue, queue) {

   226

        if (command->state != EC_CMD_QUEUED) continue;

   227

   228

        // Passt das aktuelle Kommando noch in den aktuellen Rahmen?

   229

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   230

            + EC_COMMAND_FOOTER_SIZE;

   231

        if (cur_data - frame_data + command_size > EC_MAX_FRAME_SIZE) break;

   232

   233

        command->state = EC_CMD_SENT;

   234

        command->index = master->command_index++;

   235

   236

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

   237

            EC_DBG("adding command 0x%02X\n", command->index);

   238

   239

        // Command-Following-Flag im letzten Kommando setzen

   240

        if (follows_word)

   241

            EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);

   242

   243

        // EtherCAT command header

   244

        EC_WRITE_U8 (cur_data,     command->type);

   245

        EC_WRITE_U8 (cur_data + 1, command->index);

   246

        EC_WRITE_U32(cur_data + 2, command->address.logical);

   247

        EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);

   248

        EC_WRITE_U16(cur_data + 8, 0x0000);

   249

        follows_word = cur_data + 6;

   250

        cur_data += EC_COMMAND_HEADER_SIZE;

   251

   252

        // EtherCAT command data

   253

        memcpy(cur_data, command->data, command->data_size);

   254

        cur_data += command->data_size;

   255

   256

        // EtherCAT command footer

   257

        EC_WRITE_U16(cur_data, 0x0000); // Working counter

   258

        cur_data += EC_COMMAND_FOOTER_SIZE;

   259

    }

   260

   261

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   262

        if (unlikely(master->debug_level > 0)) EC_DBG("nothing to send.\n");

   263

        return;

   264

    }

   265

   266

    // EtherCAT frame header

   267

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   268

                               - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);

   269

   270

    // Rahmen auffüllen

   271

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   272

        EC_WRITE_U8(cur_data++, 0x00);

   273

   274

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

   275

        EC_DBG("Frame size: %i\n", cur_data - frame_data);

   276

   277

    // Send frame

   278

    ec_device_send(master->device, cur_data - frame_data);

   279

   280

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

   281

        end = get_cycles();

   282

        EC_DBG("ec_master_send finished in %ius.\n",

   283

               (u32) (end - start) * 1000 / cpu_khz);

   284

    }

   285

}

   286

   287

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

   288

   289

/**

   290

   Wertet einen empfangenen Rahmen aus.

   291

   292

   \return 0 bei Erfolg, sonst < 0

   293

*/

   294

   295

void ec_master_receive(ec_master_t *master, /**< EtherCAT-Master */

   296

                      const uint8_t *frame_data, /**< Empfangene Daten */

   297

                      size_t size /**< Anzahl empfangene Datenbytes */

   298

                      )

   299

{

   300

    size_t frame_size, data_size;

   301

    uint8_t command_type, command_index;

   302

    unsigned int cmd_follows, matched;

   303

    const uint8_t *cur_data;

   304

    ec_command_t *command;

   305

   306

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   307

        master->stats.corrupted++;

   308

        ec_master_output_stats(master);

   309

        return;

   310

    }

   311

   312

    cur_data = frame_data;

   313

   314

    // Länge des gesamten Frames prüfen

   315

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   316

    cur_data += EC_FRAME_HEADER_SIZE;

   317

   318

    if (unlikely(frame_size > size)) {

   319

        master->stats.corrupted++;

   320

        ec_master_output_stats(master);

   321

        return;

   322

    }

   323

   324

    cmd_follows = 1;

   325

    while (cmd_follows) {

   326

        // Kommando-Header auswerten

   327

        command_type  = EC_READ_U8 (cur_data);

   328

        command_index = EC_READ_U8 (cur_data + 1);

   329

        data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;

   330

        cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;

   331

        cur_data += EC_COMMAND_HEADER_SIZE;

   332

   333

        if (unlikely(cur_data - frame_data

   334

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   335

            master->stats.corrupted++;

   336

            ec_master_output_stats(master);

   337

            return;

   338

        }

   339

   340

        // Suche passendes Kommando in der Liste

   341

        matched = 0;

   342

        list_for_each_entry(command, &master->command_queue, queue) {

   343

            if (command->state == EC_CMD_SENT

   344

                && command->type == command_type

   345

                && command->index == command_index

   346

                && command->data_size == data_size) {

   347

                matched = 1;

   348

                break;

   349

            }

   350

        }

   351

   352

        // Kein passendes Kommando in der Liste gefunden

   353

        if (!matched) {

   354

            master->stats.unmatched++;

   355

            ec_master_output_stats(master);

   356

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   357

            continue;

   358

        }

   359

   360

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   361

        memcpy(command->data, cur_data, data_size);

   362

        cur_data += data_size;

   363

   364

        // Working-Counter setzen

   365

        command->working_counter = EC_READ_U16(cur_data);

   366

        cur_data += EC_COMMAND_FOOTER_SIZE;

   367

   368

        // Kommando aus der Warteschlange entfernen

   369

        command->state = EC_CMD_RECEIVED;

   370

        list_del_init(&command->queue);

   371

    }

   372

}

   373

   374

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

   375

   376

/**

   377

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   378

   379

   Wenn der Slave nicht antwortet, wird das Kommando

   380

   nochmals gesendet.

   381

   382

   \return 0 bei Erfolg, sonst < 0

   383

*/

   384

   385

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

   386

                        ec_command_t *command /**< Kommando */

   387

                        )

   388

{

   389

    unsigned int response_tries_left;

   390

   391

    response_tries_left = 10;

   392

   393

    while (1)

   394

    {

   395

        ec_master_queue_command(master, command);

   396

        ecrt_master_sync_io(master);

   397

   398

        if (command->state == EC_CMD_RECEIVED) {

   399

            if (likely(command->working_counter))

   400

                return 0;

   401

        }

   402

        else if (command->state == EC_CMD_TIMEOUT) {

   403

            EC_ERR("Simple-IO TIMEOUT!\n");

   404

            return -1;

   405

        }

   406

        else if (command->state == EC_CMD_ERROR) {

   407

            EC_ERR("Simple-IO command error!\n");

   408

            return -1;

   409

        }

   410

   411

        // Keine direkte Antwort. Dem Slave Zeit lassen...

   412

        udelay(100);

   413

   414

        if (unlikely(--response_tries_left)) {

   415

            EC_ERR("No response in simple-IO!\n");

   416

            return -1;

   417

        }

   418

    }

   419

}

   420

   421

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

   422

   423

/**

   424

   Durchsucht den EtherCAT-Bus nach Slaves.

   425

   426

   Erstellt ein Array mit allen Slave-Informationen die für den

   427

   weiteren Betrieb notwendig sind.

   428

   429

   \return 0 bei Erfolg, sonst < 0

   430

*/

   431

   432

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

   433

{

   434

    ec_slave_t *slave;

   435

    ec_slave_ident_t *ident;

   436

    unsigned int i;

   437

    ec_command_t *command;

   438

    ec_eoe_t *eoe;

   439

   440

    if (master->slaves || master->slave_count) {

   441

        EC_ERR("Slave scan already done!\n");

   442

        return -1;

   443

    }

   444

   445

    command = &master->simple_command;

   446

   447

    // Determine number of slaves on bus

   448

    if (ec_command_brd(command, 0x0000, 4)) return -1;

   449

    if (unlikely(ec_master_simple_io(master, command))) return -1;

   450

    master->slave_count = command->working_counter;

   451

    EC_INFO("Found %i slaves on bus.\n", master->slave_count);

   452

   453

    if (!master->slave_count) return 0;

   454

   455

    if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   456

                                                  * sizeof(ec_slave_t),

   457

                                                  GFP_KERNEL))) {

   458

        EC_ERR("Could not allocate memory for slaves!\n");

   459

        return -1;

   460

    }

   461

   462

    // Init slaves

   463

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

   464

        slave = master->slaves + i;

   465

        ec_slave_init(slave, master);

   466

        slave->ring_position = i;

   467

        slave->station_address = i + 1;

   468

    }

   469

   470

    // For every slave in the list

   471

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

   472

    {

   473

        slave = master->slaves + i;

   474

   475

        // Write station address

   476

        if (ec_command_apwr(command, slave->ring_position,

   477

                            0x0010, sizeof(uint16_t))) return -1;

   478

        EC_WRITE_U16(command->data, slave->station_address);

   479

        if (unlikely(ec_master_simple_io(master, command))) {

   480

            EC_ERR("Writing station address failed on slave %i!\n", i);

   481

            return -1;

   482

        }

   483

   484

        // Fetch all slave information

   485

        if (ec_slave_fetch(slave)) return -1;

   486

   487

        // Search for identification in "database"

   488

        ident = slave_idents;

   489

        while (ident->type) {

   490