5

from threading import Thread

     9

from targets.typemapping import LogLevelsDict

    10

    11

SDOAnswered = PLCBinary.SDOAnswered

    12

SDOAnswered.restype = None

    13

SDOAnswered.argtypes = []

    14

    15

SDOThread = None

    16

SDOProc = None

    17

Result = None

    18

    19

def SDOThreadProc(*params):

    20

    global Result, SDOProc

    21

    if params[0] == "upload":

    22

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

    23

    else:

    24

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

    25

    26

    command = cmdfmt % params[1:]

    27

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

    28

    res = SDOProc.wait()

    29

    output = SDOProc.communicate()[0]

    30

    31

    if params[0] == "upload":

    32

        Result = None

    33

        if res == 0:

    34

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

    35

                Result = float(output)

    36

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

    37

                Result = output

    38

            else:

    39

                hex_value, dec_value = output.split()

    40

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

    41

                    Result = int(dec_value)

    42

    else:

    43

        Result = res == 0

    44

    45

    SDOAnswered()

    46

    if res != 0 :

    47

        PLCObject.LogMessage(

    48

            LogLevelsDict["WARNING"],

    49

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

    50

    51

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

    52

    global SDOThread

    53

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

    54

    SDOThread.start()

    55

    56

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

    57

    global SDOThread

    58

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

    59

    SDOThread.start()

    60

    61

def GetResult():

    62

    global Result

    63

    return Result

    64

    65

KMSGPollThread=None

    66

StopKMSGThread=False

    67

def KMSGPollThreadProc():

    68

    """

    69

    Logs Kernel messages starting with EtherCAT

    70

    Uses GLibc wrapper to Linux syscall "klogctl"

    71

    Last 4 KB are polled, and lines compared to last

    72

    captured line to detect new lines

    73

    """

    74

    global StopKMSGThread

    75

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

    76

    klog = libc.klogctl

    77

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

    78

    klog.restype = ctypes.c_int

    79

    s=ctypes.create_string_buffer(4*1024)

    80

    last = None

    81

    while not StopKMSGThread:

    82

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

    83

        log = s.value[:l-1]

    84

        if last :

    85

            log = log.rpartition(last)[2]

    86

        if log :

    87

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

    88

            for lvl,msg in re.findall(

    89

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

    90

                            log, re.MULTILINE):

    91

                PLCObject.LogMessage(

    92

                    LogLevelsDict[{

    93

                        "4":"WARNING",

    94

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

    95

                    msg)

    96

        time.sleep(0.5)

    97

    98

def _runtime_etherlab_init():

    99

    global KMSGPollThread, StopKMSGThread

   100

    StopKMSGThread = False

   101

    KMSGPollThread = Thread(target = KMSGPollThreadProc)

   102

    KMSGPollThread.start()

   103

   104

def _runtime_etherlab_cleanup():

   105

    global KMSGPollThread, StopKMSGThread, SDOProc, SDOThread

   106

    try:

   107

        os.kill(SDOProc.pid, SIGTERM)

   108

    except Exception:

   109

        pass

   110

    SDOThread = None

   111

    StopKMSGThread = True

   112

    KMSGPollThread = None