1

from __future__ import absolute_import

     2

import os

     3

import signal

     4

import subprocess

     5

import ctypes

     6

from threading import Thread

     7

import time

     8

import re

     9

    10

import runtime.PLCObject as PLCObject

    11

from runtime.loglevels import LogLevelsDict

    12

    13

SDOAnswered = PLCBinary.SDOAnswered

    14

SDOAnswered.restype = None

    15

SDOAnswered.argtypes = []

    16

    17

SDOThread = None

    18

SDOProc = None

    19

Result = None

    20

    21

    22

def SDOThreadProc(*params):

    23

    global Result, SDOProc

    24

    if params[0] == "upload":

    25

        cmdfmt = "ethercat upload -p %d -t %s 0x%.4x 0x%.2x"

    26

    else:

    27

        cmdfmt = "ethercat download -p %d -t %s 0x%.4x 0x%.2x %s"

    28

    29

    command = cmdfmt % params[1:]

    30

    SDOProc = subprocess.Popen(command, stdout=subprocess.PIPE, shell=True)

    31

    res = SDOProc.wait()

    32

    output = SDOProc.communicate()[0]

    33

    34

    if params[0] == "upload":

    35

        Result = None

    36

        if res == 0:

    37

            if params[2] in ["float", "double"]:

    38

                Result = float(output)

    39

            elif params[2] in ["string", "octet_string", "unicode_string"]:

    40

                Result = output

    41

            else:

    42

                hex_value, dec_value = output.split()

    43

                if int(hex_value, 16) == int(dec_value):

    44

                    Result = int(dec_value)

    45

    else:

    46

        Result = res == 0

    47

    48

    SDOAnswered()

    49

    if res != 0:

    50

        PLCObject.LogMessage(

    51

            LogLevelsDict["WARNING"],

    52

            "%s : %s" % (command, output))

    53

    54

    55

def EthercatSDOUpload(pos, index, subindex, var_type):

    56

    global SDOThread

    57

    SDOThread = Thread(target=SDOThreadProc, args=["upload", pos, var_type, index, subindex])

    58

    SDOThread.start()

    59

    60

    61

def EthercatSDODownload(pos, index, subindex, var_type, value):

    62

    global SDOThread

    63

    SDOThread = Thread(target=SDOThreadProc, args=["download", pos, var_type, index, subindex, value])

    64

    SDOThread.start()

    65

    66

    67

def GetResult():

    68

    global Result

    69

    return Result

    70

    71

    72

KMSGPollThread = None

    73

StopKMSGThread = False

    74

    75

    76

def KMSGPollThreadProc():

    77

    """

    78

    Logs Kernel messages starting with EtherCAT

    79

    Uses GLibc wrapper to Linux syscall "klogctl"

    80

    Last 4 KB are polled, and lines compared to last

    81

    captured line to detect new lines

    82

    """

    83

    global StopKMSGThread

    84

    libc = ctypes.CDLL("libc.so.6")

    85

    klog = libc.klogctl

    86

    klog.argtypes = [ctypes.c_int, ctypes.c_char_p, ctypes.c_int]

    87

    klog.restype = ctypes.c_int

    88

    s = ctypes.create_string_buffer(4*1024)

    89

    last = None

    90

    while not StopKMSGThread:

    91

        bytes_to_read = klog(3, s, len(s)-1)

    92

        log = s.value[:bytes_to_read-1]

    93

        if last:

    94

            log = log.rpartition(last)[2]

    95

        if log:

    96

            last = log.rpartition('\n')[2]

    97

            for lvl, msg in re.findall(

    98

                    r'<(\d)>\[\s*\d*\.\d*\]\s*(EtherCAT\s*.*)$',

    99

                    log, re.MULTILINE):

   100

                PLCObject.LogMessage(

   101

                    LogLevelsDict[{

   102

                        "4": "WARNING",

   103

                        "3": "CRITICAL"}.get(lvl, "DEBUG")],

   104

                    msg)

   105

        time.sleep(0.5)

   106

   107

   108

def _runtime_etherlab_init():

   109

    global KMSGPollThread, StopKMSGThread

   110

    StopKMSGThread = False

   111

    KMSGPollThread = Thread(target=KMSGPollThreadProc)

   112

    KMSGPollThread.start()

   113

   114

   115

def _runtime_etherlab_cleanup():

   116

    global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread

   117

    try:

   118

        os.kill(SDOProc.pid, signal.SIGTERM)

   119

    except Exception:

   120

        pass

   121

    SDOThread = None

   122

    StopKMSGThread = True

   123

    KMSGPollThread = None