PoKeysLibCore.c

/*

Copyright (C) 2013 Matev� Bo�nak (matevz@poscope.com)

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA 

*/

#include <stdlib.h>
#include "PoKeysLibHal.h"
#include "PoKeysLibCore.h"
#include "PoKeysLibCoreSockets.h"
#include "string.h"
#include "stdio.h"
//#define PK_COM_DEBUG

/**
 * @file PoKeysLibCore.c
 * @brief Core communication helpers for the PoKeys protocol.
 *
 * This module implements basic request formatting and transport
 * functions used by almost all higher level commands. It covers USB,
 * fast USB and Ethernet communication as described in the PoKeys
 * protocol specification.
 */

/**
 * @brief Validate HID interface as PoKeys communication channel.
 *
 * The PoKeys USB devices expose multiple HID interfaces. Only the
 * interface with index 1 carries the standard command protocol used by
 * this library. This helper checks the passed HID interface information
 * and reports whether it should be used for communication.
 *
 * @param devInfo HID device information structure obtained from
 *                hid_enumerate().
 * @return 1 when the interface number matches the communication
 *         interface, otherwise 0.
 */
int32_t PKI_CheckInterface(struct hid_device_info * devInfo)
{
    if (devInfo->interface_number == 1)
        return 1;

    return 0;
}

#ifndef RTAPI
// Connection specific commands
/**
 * @brief Enumerate PoKeys USB devices.
 *
 * Two product identifiers are scanned (0x1001 and 0x1002) under the
 * PoLabs vendor ID 0x1DC3. Interfaces are filtered through
 * PKI_CheckInterface() so that only the communication interface (index 1)
 * is counted for devices using the standard HID protocol. For the older
 * fast USB interface the interface number is -1.
 *
 * @return Number of PoKeys devices detected on the USB bus.
 */
int32_t PK_EnumerateUSBDevices()
{
    int32_t numDevices = 0;
    struct hid_device_info *devs, *cur_dev;
#ifndef RTAPI
    devs = hid_enumerate(0x1DC3, 0x1001);
    #endif
    cur_dev = devs;

    while (cur_dev)
    {
        /*printf("Device Found\n");
        printf("  Serial:      %ls\n", cur_dev->serial_number);
        printf("  Product:      %ls\n", cur_dev->product_string);
        printf("  Interface:    %d\n",  cur_dev->interface_number);
        printf("\n");*/
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device Found, Serial: %ls, Product: %ls, Interface: %d\n", __FILE__, __FUNCTION__, cur_dev->serial_number, cur_dev->product_string, cur_dev->interface_number);
        if (PKI_CheckInterface(cur_dev))
        {
            numDevices++;
        }

        cur_dev = cur_dev->next;
    }
    
    hid_free_enumeration(devs);
    #ifndef RTAPI
    devs = hid_enumerate(0x1DC3, 0x1002);
    #endif
    cur_dev = devs;

    while (cur_dev)
    {
        /*printf("Device Found\n");
        printf("  Serial:      %ls\n", cur_dev->serial_number);
        printf("  Product:      %ls\n", cur_dev->product_string);
        printf("  Interface:    %d\n",  cur_dev->interface_number);
        printf("\n");*/
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device Found, Serial: %ls, Product: %ls, Interface: %d\n", __FILE__, __FUNCTION__, cur_dev->serial_number, cur_dev->product_string, cur_dev->interface_number);
        if (cur_dev->interface_number == -1) numDevices++;
        cur_dev = cur_dev->next;
    }

    hid_free_enumeration(devs);


#ifdef POKEYSLIB_USE_LIBUSB
	numDevices += PK_EnumerateFastUSBDevices();
#endif
    return numDevices;
}
#else
/**
 * @brief Stub for RTAPI builds.
 *
 * USB enumeration is handled by the realtime layer so this function
 * simply returns PK_OK.
 */
int32_t PK_EnumerateUSBDevices()
{
    return PK_OK;
}
#endif
/**
 * @brief Obtain the active connection type for a device.
 *
 * The field ::connectionType of the device structure holds the current
 * transport that is used for communication. Possible values are defined
 * by ::ePK_DeviceConnectionType (for example
 * PK_DeviceType_USBDevice or PK_DeviceType_NetworkDevice).
 *
 * @param device Pointer to an initialised PoKeys device structure.
 * @return The current connection type constant.
 */
int32_t PK_GetCurrentDeviceConnectionType(sPoKeysDevice* device)
{
        return device->connectionType;
}

void InitializeNewDevice(sPoKeysDevice* device)
{
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing new device...\n", __FILE__, __FUNCTION__);
    uint32_t i;    
	memset(&device->info, 0, sizeof(sPoKeysDevice_Info));
    memset(&device->DeviceData, 0, sizeof(sPoKeysDevice_Data));

    device->netDeviceData = 0;

	memset(&device->matrixKB, 0, sizeof(sMatrixKeyboard));
	memset(&device->PWM, 0, sizeof(sPoKeysPWM));
	memset(&device->LCD, 0, sizeof(sPoKeysLCD));
	

	device->FastEncodersConfiguration = 0;
	device->FastEncodersOptions = 0;
	device->UltraFastEncoderConfiguration = 0;
	device->UltraFastEncoderOptions = 0;

	device->UltraFastEncoderFilter = 0;

	memset(device->request, 0, 64);
	memset(device->response, 0, 64);

	device->sendRetries = 3;
    device->readRetries = 10;
    device->socketTimeout = 100;

    PK_DeviceDataGet(device);

    device->Pins = (sPoKeysPinData*)hal_malloc(sizeof(sPoKeysPinData) * device->info.iPinCount);
	memset(device->Pins, 0, sizeof(sPoKeysPinData) * device->info.iPinCount);

    device->AnalogInput = (sPoKeysAnalogData*)hal_malloc(sizeof(sPoKeysAnalogData) * 7);
    memset(device->AnalogInput, 0, sizeof(sPoKeysAnalogData) * 7);

	for (i = 0; i < device->info.iPinCount; i++)
	{
		if (PK_IsCounterAvailable(device, i))
		{
			device->Pins[i].DigitalCounterAvailable = 1;
		} else
		{
			device->Pins[i].DigitalCounterAvailable = 0;
		}
	}

    // Allocate memory for encoders
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device iEncodersCount: %d\n", __FILE__, __FUNCTION__, device->info.iEncodersCount);
	device->Encoders = (sPoKeysEncoder*)hal_malloc(sizeof(sPoKeysEncoder) * device->info.iEncodersCount);
	memset(device->Encoders, 0, sizeof(sPoKeysEncoder) * device->info.iEncodersCount);

    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device iEasySensors: %d\n", __FILE__, __FUNCTION__, device->info.iEasySensors);
    if (device->info.iEasySensors)
    {
        device->EasySensors = (sPoKeysEasySensor*)hal_malloc(sizeof(sPoKeysEasySensor) * device->info.iEasySensors);
        memset(device->EasySensors, 0, sizeof(sPoKeysEasySensor) * device->info.iEasySensors);
    } else
    {
        device->EasySensors = NULL;
    }

    /**
    General pin settings
        PoKeys55, PoKeys56, PoKeys57 limitations
        1. Pin codes used in PoKeys55 device are 0-based, e.g. pin 1 has pin code of 0, pin 55 has pin code of 54.
        2. Analog input capable pins 43 to 47 have pin codes of 42 to 46.
        3. Analog output capable pin 43 has pin code of 42.
        4. PWM (pulse-width modulation) capable pins 17 to 22 have pin codes of 16-21 (PWM module outputs are in reversed order, e.g. pin 17 (pin coded as 16) is connected to PWM6 output – see specifications below).
    */
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device iPWMCount: %d\n", __FILE__, __FUNCTION__, device->info.iPinCount);
	if (device->info.iPWMCount > 0)
	{
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Allocating memory for PWM.max_Voltage...\n", __FILE__, __FUNCTION__);
        device->PWM.max_Voltage = (hal_float_t*)hal_malloc(sizeof(hal_float_t) * device->info.iPWMCount);
        for (uint32_t i = 0; i < device->info.iPWMCount; i++)
        {
            // check for first pin of PK_DeviceID_PoKeys57CNC
            if (i==5 && device->DeviceData.DeviceTypeID == PK_DeviceID_PoKeys57CNC)
            {
                /**
                PoKeys57CNC device supports PWM output function on 4 pins (pins 18, 20, 21 and 22). Different duty
                cycles can be assigned to each PWM output however, all outputs share the same PWM period. PWM
                outputs can be easily amplified using an external transistor and used for control of loads with increased
                increased current demand - pins with such function embedded are marked as OC (open-collector) in
                the pinout diagram. PoKeys PWM outputs can also be used to drive various R/C servo motors that
                accept PWM signal with 50 Hz frequency (20 ms PWM period) and duty cycles between 5 and 10 % (1
                to 2 ms).
                PoKeys devices have an in-built PWM module that operates at a fixed clock frequency (25 MHz). Both
                the PWM period and the PWM duty cycles must be expressed as number of module clock cycles (i.e.
                20 ms PWM period equates to 0.020 x 25 000 000 = 500 000)
                */
                device->PWM.max_Voltage[i] = 10.0; // Default max voltage for PWM outputs on PoKeys57CNC
            }
            else if (i==5 && device->DeviceData.DeviceTypeID == PK_DeviceID_PoKeys57CNCpro4x25)
            {
                /**
                PoKeys57CNCpro4x25 device supports PWM output function on pins 17 and 20. Pin 17 is wired to 0-
                10 V output for spindle control – the output voltage is proportional to the duty cycle of the PWM
                output. PWM output on pin 20 is located on dedicated connector and can be easily amplified using an
                external transistor and used for controlling the loads with increased current demand – we offer
                MOSFET power switch adapter for such purposes in PoLabs online store.
                The frequency of the generated PWM signal can be changed – for the analog output functionality on
                pin 17 to operate as designed, a PWM frequency of 20 kHz is suggested. However, if needed, the
                frequency of PoKeys PWM outputs can be reduced – e.g. to drive various R/C servo motors that accept
                PWM signal with 50 Hz frequency (20 ms PWM period) and duty cycles between 5 and 10 % (1 to 2
                ms). In such case, analog output on pin 17 will not be operational.
                PoKeys devices have a built-in PWM module that operates at a fixed clock frequency of 25 MHz. Both
                the PWM period and the PWM duty cycles must be expressed as number of module clock cycles (i.e.
                20 ms PWM period equates to 0.020 x 25 000 000 = 500 000).
                */
                device->PWM.max_Voltage[i] = 10.0; // Default max voltage for PWM outputs on PoKeys57CNC
            }
            else{
                // Default max voltage for other PWM outputs
                device->PWM.max_Voltage[i] = 5.0; // Default max voltage for PWM outputs
            }
            
        }

     /* that is done in hal_export_adcout
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Allocating memory for PWM.PWMduty...\n", __FILE__, __FUNCTION__);
        device->PWM.PWMduty = (hal_u32_t*)hal_malloc(sizeof(hal_u32_t) * device->info.iPWMCount);
        //memset(device->PWM.PWMduty, 0, sizeof(uint32_t) * device->info.iPWMCount);
        for (uint32_t i = 0; i < device->info.iPWMCount; i++)
            device->PWM.PWMduty[i] = 0;
    */
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Allocating memory for PWM.PWManalogOutputs...\n", __FILE__, __FUNCTION__);
        device->PWM.PWManalogOutputs = (hal_adcout_t*)hal_malloc(sizeof(hal_adcout_t) * device->info.iPWMCount);
        if (device->PWM.PWMperiod==0000){
            device->PWM.PWMperiod = 500000; // Default PWM period in clock cycles (20 ms at 25 MHz)
        }
        for (uint32_t i = 0; i < device->info.iPWMCount; i++)
        {
          //  rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].value\n", __FILE__, __FUNCTION__, i);
          //  device->PWM.PWManalogOutputs[i].value = (hal_float_t*)hal_malloc(sizeof(hal_float_t));
          //  *(device->PWM.PWManalogOutputs[i].value) = 0.0;
          //  rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].enable\n", __FILE__, __FUNCTION__, i);
          //  device->PWM.PWManalogOutputs[i].enable = (hal_bit_t *)hal_malloc(sizeof(hal_bit_t));
          //  *(device->PWM.PWManalogOutputs[i].enable) = 0;
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].offset\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].offset = 0.0;
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].scale\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].scale = 1.0;
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].high_limit\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].high_limit = device->PWM.max_Voltage[i];
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].low_limit\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].low_limit = 0.0;
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].bit_weight\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].bit_weight = device->PWM.max_Voltage[i] / device->PWM.PWMperiod;
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Initializing PWM.PWManalogOutputs[%d].hw_offset\n", __FILE__, __FUNCTION__, i);
            device->PWM.PWManalogOutputs[i].hw_offset = 0.0;
        }

        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Allocating memory for PWM.PWMenabledChannels...\n", __FILE__, __FUNCTION__);
		device->PWM.PWMenabledChannels = (unsigned char*)hal_malloc(sizeof(unsigned char) * device->info.iPWMCount);
		//memset(device->PWM.PWMenabledChannels, 0, sizeof(unsigned char) * device->info.iPWMCount);
        for (uint32_t i = 0; i < device->info.iPWMCount; i++)
            device->PWM.PWMenabledChannels[i] = 0;
	}
	else
	{
		device->PWM.PWMenabledChannels = NULL;
	}
	device->PWM.PWMpinIDs = (unsigned char*)hal_malloc(sizeof(unsigned char) * device->info.iPWMCount);
	memset(device->PWM.PWMpinIDs, 0, sizeof(unsigned char) * device->info.iPWMCount);

	PK_FillPWMPinNumbers(device);

	device->PoExtBusData = (unsigned char*)hal_malloc(sizeof(unsigned char) * device->info.iPoExtBus);

    device->MatrixLED = (sPoKeysMatrixLED*)hal_malloc(sizeof(sPoKeysMatrixLED) * device->info.iMatrixLED);
	memset(device->MatrixLED, 0, sizeof(sPoKeysMatrixLED) * device->info.iMatrixLED);

    memset(&device->PEv2, 0, sizeof(sPoKeysPEv2));

    device-> multiPartBuffer = hal_malloc(512);
    if (device->multiPartBuffer <= 0) device->multiPartBuffer = 0;

#ifdef USE_ALIGN_TEST
    device->alignTest1 = 1;
    device->alignTest2 = 2;
    device->alignTest3 = 3;
    device->alignTest4 = 4;
    device->alignTest5 = 5;
    device->alignTest6 = 6;
    device->alignTest7 = 7;
    device->alignTest8 = 8;
    device->alignTest9 = 9;
    device->alignTest10 = 10;
    device->alignTest11 = 11;
#endif
rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Device initialized with %d pins, %d encoders, %d PWM channels, %d matrix LEDs\n", __FILE__, __FUNCTION__, device->info.iPinCount, device->info.iEncodersCount, device->info.iPWMCount, device->info.iMatrixLED);
}

void CleanDevice(sPoKeysDevice* device)
{    
    //in HAL there is no free. The entire HAL shared memory area is freed when the last component calls hal_exit()
	//free(device->Pins);
	device->Pins = NULL;
	//free(device->Encoders);
	device->Encoders = NULL;
	////free(device->PWM.PWMduty); in HAL there is no free. The entire HAL shared memory area is freed when the last component calls hal_exit()
	*(device->PWM.PWMduty) = NULL;
	//free(device->PWM.PWMenabledChannels);
	device->PWM.PWMenabledChannels = NULL;
	//free(device->PWM.PWMpinIDs);
	device->PWM.PWMpinIDs = NULL;
	//free(device->PoExtBusData);
	device->PoExtBusData = NULL;
	//free(device->MatrixLED);
	device->MatrixLED = NULL;

    if (device->multiPartBuffer != NULL)
    {
        //free(device->multiPartBuffer);
        device->multiPartBuffer = NULL;
    }

    if (device->EasySensors != NULL)
    {
        //free(device->EasySensors);
        device->EasySensors = NULL;
    }

    if (device->netDeviceData != NULL)
    {
        //free(device->netDeviceData);
        device->netDeviceData = NULL;
    }
}

void PK_ReleaseDeviceStructure(sPoKeysDevice* device)
{
    CleanDevice(device);
}

void PK_CloneDeviceStructure(sPoKeysDevice* original, sPoKeysDevice *destination)
{
    // Reserve memory...
    destination->Pins = (sPoKeysPinData*)hal_malloc(sizeof(sPoKeysPinData) * original->info.iPinCount);
    destination->Encoders = (sPoKeysEncoder*)hal_malloc(sizeof(sPoKeysEncoder) * original->info.iEncodersCount);
    *(destination->PWM.PWMduty) = (uint32_t*)hal_malloc(sizeof(uint32_t) * original->info.iPWMCount);
    destination->PWM.PWMenabledChannels = (unsigned char*)hal_malloc(sizeof(unsigned char) * original->info.iPWMCount);
	if (original->info.iPWMCount == 0) destination->PWM.PWMenabledChannels = NULL;
    destination->PWM.PWMpinIDs = (unsigned char*)hal_malloc(sizeof(unsigned char) * original->info.iPWMCount);
    destination->MatrixLED = (sPoKeysMatrixLED*)hal_malloc(sizeof(sPoKeysMatrixLED) * original->info.iMatrixLED);

    if (original->info.iEasySensors)
    {
        destination->EasySensors = (sPoKeysEasySensor*)hal_malloc(sizeof(sPoKeysEasySensor) * original->info.iEasySensors);
    } else
    {
        destination->EasySensors = 0;
    }

    // Network device information structure...
    if (original->netDeviceData != 0)
    {
        destination->netDeviceData = (sPoKeysNetworkDeviceInfo *)hal_malloc(sizeof(sPoKeysNetworkDeviceInfo));
        memcpy(destination->netDeviceData, original->netDeviceData, sizeof(sPoKeysNetworkDeviceInfo));
    } else
    {
        destination->netDeviceData = 0;
    }
    destination->PoExtBusData = (unsigned char*)hal_malloc(sizeof(unsigned char) * original->info.iPoExtBus);


    // Copy data
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Copying devHandle (dest:%d org: %d)\n", __FILE__, __FUNCTION__ ,destination->devHandle, original->devHandle );
    destination->devHandle = original->devHandle;
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Copying devHandle2 (dest:%d org: %d)\n", __FILE__, __FUNCTION__ ,destination->devHandle2, original->devHandle2 );
    destination->devHandle2 = original->devHandle2;

    destination->info = original->info;
    destination->DeviceData = original->DeviceData;

    memcpy(&destination->Pins[0], &original->Pins[0],
            original->info.iPinCount * sizeof(sPoKeysPinData));
    memcpy(&destination->Encoders[0], &original->Encoders[0],
            original->info.iEncodersCount * sizeof(sPoKeysEncoder));


    if (original->info.iEasySensors)
    {
        memcpy(&destination->EasySensors[0], &original->EasySensors[0],
                original->info.iEasySensors * sizeof(sPoKeysEasySensor));
    }

    destination->matrixKB = original->matrixKB;

    destination->PWM.PWMperiod = original->PWM.PWMperiod;
    *(destination->PWM.PWMduty) = *(original->PWM.PWMduty);
    
    memcpy(destination->PWM.PWMenabledChannels, original->PWM.PWMenabledChannels,
           sizeof(unsigned char) * original->info.iPWMCount);
	memcpy(destination->PWM.PWMpinIDs, original->PWM.PWMpinIDs,
           sizeof(unsigned char) * original->info.iPWMCount);

    memcpy(destination->MatrixLED, original->MatrixLED,
           sizeof(sPoKeysMatrixLED) * original->info.iMatrixLED);

    destination->LCD = original->LCD;

    destination->PoNETmodule = original->PoNETmodule;
    destination->PoIL = original->PoIL;
    destination->RTC = original->RTC;

    destination->FastEncodersConfiguration =    original->FastEncodersConfiguration;
    destination->FastEncodersOptions =          original->FastEncodersOptions;
    destination->UltraFastEncoderConfiguration =original->UltraFastEncoderConfiguration;
    destination->UltraFastEncoderOptions =      original->UltraFastEncoderOptions;
    destination->UltraFastEncoderFilter =       original->UltraFastEncoderFilter;

    memcpy(destination->PoExtBusData, original->PoExtBusData, sizeof(unsigned char) * original->info.iPoExtBus);

    destination->connectionType = original->connectionType;
    destination->requestID = original->requestID;

}

void * PK_FastUSBConnectToDevice(uint32_t deviceIndex);

sPoKeysDevice* PK_ConnectToDevice(uint32_t deviceIndex)
{
    int32_t numDevices = 0;
    struct hid_device_info *devs, *cur_dev;
    sPoKeysDevice* tmpDevice;
    void * devData;
#ifndef RTAPI
    devs = hid_enumerate(0x1DC3, 0x1001);
    #endif
    cur_dev = devs;

    while (cur_dev)
    {
        if (cur_dev->interface_number == 1)
        {
            if (numDevices == deviceIndex)
            {
				tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));

                //printf("Connect to this device...");
                rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Connect to this device...  (devHandle:%d devHandle2:%d)\n", __FILE__, __FUNCTION__,tmpDevice->devHandle,tmpDevice->devHandle2);
                #ifndef RTAPI
				tmpDevice->devHandle = (void*)hid_open_path(cur_dev->path);
                #endif
                tmpDevice->devHandle2 = NULL;
                rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: (devHandle:%d devHandle2:%d)\n", __FILE__, __FUNCTION__,tmpDevice->devHandle,tmpDevice->devHandle2);

				tmpDevice->connectionType = PK_DeviceType_USBDevice;


				if (tmpDevice->devHandle != NULL)
				{
                    InitializeNewDevice(tmpDevice);
				} else
				{
					//free(tmpDevice);
					tmpDevice = NULL;
				}
                //hid_set_nonblocking(devHandle);
                #ifndef RTAPI
                hid_free_enumeration(devs);
                #endif
                return tmpDevice;
            }
            numDevices++;
        }
        cur_dev = cur_dev->next;
    }
    #ifndef RTAPI
    hid_free_enumeration(devs);
    #endif
#ifndef RTAPI
    // Continue with 0x1002 devices
    devs = hid_enumerate(0x1DC3, 0x1002);
#endif
    cur_dev = devs;

    while (cur_dev)
    {
        if (cur_dev->interface_number == -1)
        {
            if (numDevices == deviceIndex)
            {
                tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));

                //printf("Connect to this device...");
                rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: (devHandle:%d devHandle2:%d)\n", __FILE__, __FUNCTION__,tmpDevice->devHandle,tmpDevice->devHandle2);
                #ifndef RTAPI
                tmpDevice->devHandle = (void*)hid_open_path(cur_dev->path);
                #endif
                tmpDevice->devHandle2 = NULL;
                rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: (devHandle:%d devHandle2:%d)\n", __FILE__, __FUNCTION__,tmpDevice->devHandle,tmpDevice->devHandle2);
               
                tmpDevice->connectionType = PK_DeviceType_USBDevice;

                if (tmpDevice->devHandle != NULL)
                {
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Connect to this device...(devHandle:%d devHandle2:%d) \n", __FILE__, __FUNCTION__, tmpDevice->devHandle,tmpDevice->devHandle2); 
                    InitializeNewDevice(tmpDevice);
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Connect to this device...(devHandle:%d devHandle2:%d) \n", __FILE__, __FUNCTION__, tmpDevice->devHandle,tmpDevice->devHandle2); 
                } else
                {
                    //free(tmpDevice);
                    tmpDevice = NULL;
                }
                //hid_set_nonblocking(devHandle);
                #ifndef RTAPI
                hid_free_enumeration(devs);
                #endif
                return tmpDevice;
            }
            numDevices++;
        }
        cur_dev = cur_dev->next;
    }
    #ifndef RTAPI
    hid_free_enumeration(devs);
    #endif

#ifdef POKEYSLIB_USE_LIBUSB
	// Try connecting to the bulk interface of the PoKeys device...
    devData = PK_FastUSBConnectToDevice(deviceIndex - numDevices);

	//void * devData = ConnectToFastUSBInterface(serialNumber);
	if (devData != NULL)
	{
		tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));

		tmpDevice->devHandle = NULL;
		tmpDevice->devHandle2 = devData;

		tmpDevice->connectionType = PK_DeviceType_FastUSBDevice;
		InitializeNewDevice(tmpDevice);
		return tmpDevice;
	}
#endif
    return NULL;
}

// Flags:
// bits 7-1: deviceType specifier (2 - PoKeys56, 3 - PoKeys58, 4 - PoKeys16)
#ifndef RTAPI
sPoKeysDevice* PK_ConnectToPoKeysDevice_USB(uint32_t serialNumber, uint32_t flags)
{
    int32_t numDevices = 0;
    struct hid_device_info *devs, *cur_dev;
    int32_t k;
    sPoKeysDevice* tmpDevice;
    uint8_t serialSearch[8];

    int devRange = 0;
    uint8_t deviceTypeRequested = (flags >> 1) & 0x7F;

#ifdef POKEYSLIB_USE_LIBUSB
	// Try connecting to fast USB interface first
	void * devData = ConnectToFastUSBInterface(serialNumber);
	if (devData != NULL)
	{
		tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));

		tmpDevice->devHandle = NULL;
        tmpDevice->devHandle2 = devData;

        tmpDevice->connectionType = PK_DeviceType_FastUSBDevice;
        InitializeNewDevice(tmpDevice);	
		return tmpDevice;
	}
#endif
#ifndef RTAPI
    devs = hid_enumerate(0x1DC3, 0x1001);
#endif
    cur_dev = devs;

    //sprintf((char*)serialSearch, "x.%05u", serialNumber % 100000);
    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Searching for device with serial number %s\n", __FILE__, __FUNCTION__, serialSearch);
    //sprintf(serialSearch58, "3.%05u", serialNumber % 100000);

    while (cur_dev)
    {
        if ((cur_dev->interface_number == 1 && devRange == 0) ||
            (cur_dev->interface_number == 0 && devRange == 1))
        {
            if (cur_dev->serial_number != 0 && cur_dev->serial_number[0] != 'P')
            {
                // Check the serial number first
                for (k = 1; k < 8 && cur_dev->serial_number[k] != 0; k++)
                {
                    if (cur_dev->serial_number[k] != serialSearch[k]) break;
                }

                if (deviceTypeRequested == 2 && cur_dev->serial_number[0] != '2') k = 0;
                if (deviceTypeRequested == 3 && cur_dev->serial_number[0] != '3') k = 0;
                if (deviceTypeRequested == 4 && cur_dev->serial_number[0] != '4') k = 0;

                if (k == 7)
                {
                    tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));

                    //printf("Connect to this device...");
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Connect to this device...(devHandle: %d  devHandle2: %d )\n", __FILE__, __FUNCTION__, tmpDevice->devHandle, tmpDevice->devHandle2);
                    #ifndef RTAPI
                    tmpDevice->devHandle = (void*)hid_open_path(cur_dev->path);
                    #endif
                    tmpDevice->devHandle2 = 0;
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: ( hid_open_path devHandle: %d  devHandle2: %d )\n", __FILE__, __FUNCTION__, tmpDevice->devHandle, tmpDevice->devHandle2);

                    tmpDevice->connectionType = PK_DeviceType_USBDevice;
                    if (tmpDevice->devHandle != NULL)
                    {
                        InitializeNewDevice(tmpDevice);
					}
					else
                    {
                        //free(tmpDevice);
                        tmpDevice = NULL;
                    }
                    //hid_set_nonblocking(devHandle);
                    #ifndef RTAPI
                    hid_free_enumeration(devs);
                    #endif
                    return tmpDevice;
                }
			}
			else
            {
                // Old, PoKeys55 device - we must to connect and read the serial number...
                tmpDevice = (sPoKeysDevice*)hal_malloc(sizeof(sPoKeysDevice));
                #ifndef RTAPI
                tmpDevice->devHandle = (void*)hid_open_path(cur_dev->path);
                #endif
                tmpDevice->devHandle2 = 0;

                if (tmpDevice->devHandle != NULL)
                {
                    InitializeNewDevice(tmpDevice);
				}
				else
                {
                    //free(tmpDevice);
                    tmpDevice = NULL;
                    #ifndef RTAPI
                    hid_free_enumeration(devs);
                    #endif
                    return NULL;
                }

                tmpDevice->connectionType = PK_DeviceType_USBDevice;
                if (tmpDevice->DeviceData.SerialNumber == serialNumber)
                {
                    #ifndef RTAPI
                    hid_free_enumeration(devs);
                    #endif
                    return tmpDevice;
				}
				else
                {
                    CleanDevice(tmpDevice);
                    //free(tmpDevice);
                }
            }

            numDevices++;
        }
        cur_dev = cur_dev->next;

        if (cur_dev == NULL)
        {
            devRange++;
            switch (devRange)
            {
                case 1:
                #ifndef RTAPI
                hid_free_enumeration(devs);
                
                    devs = hid_enumerate(0x1DC3, 0x1001);
                    #endif
                    cur_dev = devs;
                    break;
            }
        }
    }
    #ifndef RTAPI
    hid_free_enumeration(devs);
    #endif
	return NULL;
}
#else
sPoKeysDevice* PK_ConnectToPoKeysDevice_USB(uint32_t serialNumber, uint32_t flags)
{
    return NULL;
}
#endif
// Flags:
// bit 0: use UDP
sPoKeysDevice* PK_ConnectToPoKeysDevice_Ethernet(uint32_t serialNumber, uint32_t checkForNetworkDevicesAndTimeout, uint32_t flags)
{
	int32_t k;
	sPoKeysDevice* tmpDevice;
	sPoKeysNetworkDeviceSummary * devices;
	int32_t iNet;

    if (checkForNetworkDevicesAndTimeout)
    {
        devices = (sPoKeysNetworkDeviceSummary*)hal_malloc(sizeof(sPoKeysNetworkDeviceSummary) * 16);
        iNet = PK_SearchNetworkDevices(devices, checkForNetworkDevicesAndTimeout, serialNumber);

        if (iNet > 16) iNet = 16;

        for (k = 0; k < iNet; k++)
        {
            //printf("\nNetwork device found, serial = %lu at %u.%u.%u.%u", devices[k].SerialNumber, devices[k].IPaddress[0], devices[k].IPaddress[1], devices[k].IPaddress[2], devices[k].IPaddress[3]);
            rtapi_print_msg(RTAPI_MSG_INFO, "PoKeys: %s:%s: Network device found, serial = %lu at %u.%u.%u.%u\n", __FILE__, __FUNCTION__, devices[k].SerialNumber, devices[k].IPaddress[0], devices[k].IPaddress[1], devices[k].IPaddress[2], devices[k].IPaddress[3]);
            if (devices[k].SerialNumber == serialNumber)
            {
                if (flags & 1) devices[k].useUDP = 1;
                tmpDevice = PK_ConnectToNetworkDevice(&devices[k]);
                if (tmpDevice == NULL)
                {
                    //CleanDevice(tmpDevice);
                    //free(tmpDevice);
                    //printf("\nProblem connecting to the device...");
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Problem connecting to the device...\n", __FILE__, __FUNCTION__);				}
				else
                {
                    //free(devices);
                    struct sockaddr_in *a = (struct sockaddr_in *)&tmpDevice->devHandle2;
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: devHandle2=%d\n", __FILE__, __FUNCTION__,tmpDevice->devHandle2);
                    InitializeNewDevice(tmpDevice);
                    
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: -> InitializeNewDevice -done : devHandle2=%d\n", __FILE__, __FUNCTION__,tmpDevice->devHandle2);
                    return tmpDevice;
                }
            }
        }
        //free(devices);
    }

    return NULL;
}


// Flags:
// bit 8: force Ethernet
// bits 7-1: deviceType specifier (2 - PoKeys56, 3 - PoKeys58, 4 - PoKeys16)
// bit 0: use UDP
sPoKeysDevice* PK_ConnectToPoKeysDevice(uint32_t serialNumber, uint32_t checkForNetworkDevicesAndTimeout, uint32_t flags)
{
	sPoKeysDevice* tmpDevice = NULL;

	// If Ethernet devices are forced and scan interval is greater than 0...
	if ((flags & (1 << 8)) && checkForNetworkDevicesAndTimeout > 0)
	{
		tmpDevice = PK_ConnectToPoKeysDevice_Ethernet(serialNumber, checkForNetworkDevicesAndTimeout, flags);
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: → PK_ConnectToPoKeysDevice_Ethernet: devHandle2=%d\n", __FILE__, __FUNCTION__,tmpDevice->devHandle2);
	}

	// Otherwise, check USB first
	if (tmpDevice == NULL)
	{
		tmpDevice = PK_ConnectToPoKeysDevice_USB(serialNumber, flags);
	}
	// Then Ethernet
	if (tmpDevice == NULL && ((flags & (1 << 8)) == 0) && checkForNetworkDevicesAndTimeout > 0)
	{
		tmpDevice = PK_ConnectToPoKeysDevice_Ethernet(serialNumber, checkForNetworkDevicesAndTimeout, flags);
        rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: → PK_ConnectToPoKeysDevice_Ethernet: devHandle2=%d\n", __FILE__, __FUNCTION__,tmpDevice->devHandle2);
	}
	return tmpDevice;
}


sPoKeysDevice* PK_ConnectToDeviceWSerial(uint32_t serialNumber, uint32_t checkForNetworkDevicesAndTimeout)
{
    return PK_ConnectToPoKeysDevice(serialNumber, checkForNetworkDevicesAndTimeout, 0);
}

sPoKeysDevice* PK_ConnectToDeviceWSerial_UDP(uint32_t serialNumber, uint32_t checkForNetworkDevicesAndTimeout)
{
    return PK_ConnectToPoKeysDevice(serialNumber, checkForNetworkDevicesAndTimeout, 1);

}

void PK_DisconnectDevice(sPoKeysDevice* device)
{
    if (device != NULL)
    {
		if (device->connectionType == PK_DeviceType_NetworkDevice) 
		{
			PK_DisconnectNetworkDevice(device);
		} else
		{
#ifdef POKEYSLIB_USE_LIBUSB
			// Disconnect the fast USB interface...
			DisconnectFromFastUSBInterface(device->devHandle2);
			device->devHandle2 = NULL;
#endif
#ifndef RTAPI
			if ((hid_device*)device->devHandle != NULL)
			{
				hid_close((hid_device*)device->devHandle);
			}
#endif
		}

		CleanDevice(device);
		//free(device);
    }
}

/**
 * @brief Compose a PoKeys request packet.
 *
 * The PoKeys protocol uses 64 byte packets. Bytes 1 to 5 hold the
 * command identifier and up to four parameters. The header byte (0xBB),
 * request ID and checksum are appended by SendRequest().
 *
 * @param request Pointer to a 64 byte buffer that will contain the
 *                command.
 * @param type    Command ID as defined by ::pokeys_command_t (PK_CMD_*).
 * @param param1  First command parameter.
 * @param param2  Second command parameter.
 * @param param3  Third command parameter.
 * @param param4  Fourth command parameter.
 * @return PK_OK on success or PK_ERR_NOT_CONNECTED when request is NULL.
 */
int32_t CreateRequest(unsigned char * request, unsigned char type, unsigned char param1, unsigned char param2, unsigned char param3, unsigned char param4)
{
    if (request == NULL) return PK_ERR_NOT_CONNECTED;

    memset(request, 0, 64);

    request[1] = type;
    request[2] = param1;
    request[3] = param2;
    request[4] = param3;
    request[5] = param4;

    return PK_OK;
}

/**
 * @brief Send an arbitrary request to the connected device.
 *
 * The helper fills the device request buffer in the same format as
 * CreateRequest() and immediately transmits it using SendRequest().
 * It is mainly used for higher level functions that do not require
 * dedicated API calls.
 *
 * @param device  Target device structure.
 * @param type    Command ID (PK_CMD_* constant).
 * @param param1  First command parameter.
 * @param param2  Second command parameter.
 * @param param3  Third command parameter.
 * @param param4  Fourth command parameter.
 * @return Result of SendRequest().
 */
int32_t PK_CustomRequest(sPoKeysDevice* device, unsigned char type, unsigned char param1, unsigned char param2, unsigned char param3, unsigned char param4)
{
    device->request[1] = type;
    device->request[2] = param1;
    device->request[3] = param2;
    device->request[4] = param3;
    device->request[5] = param4;

    return SendRequest(device);
}

uint8_t getChecksum(uint8_t * data)
{
    uint8_t temp = 0;
    uint32_t i;

    for (i = 0; i < 7; i++)
    {
            temp += data[i];
    }
    return temp;
}

int32_t LastRetryCount = 0;
int32_t LastWaitCount = 0;


//#define PK_COM_DEBUG
/**
 * @brief Send a multipart data transfer.
 *
 * Large data blocks are transferred using the command
 * ::PK_CMD_MULTIPART_PACKET. The function delegates the actual
 * transmission to the network or fast USB helpers depending on the
 * device connection type.
 *
 * The multipart buffer and packet headers must be prepared by the
 * caller in \a device before invoking this function.
 *
 * @param device Target device instance.
 * @return PK_OK on success or an error code on failure.
 */
int32_t SendRequest_multiPart(sPoKeysDevice* device)
{
    if (device == NULL) return PK_ERR_GENERIC;
	if (device->connectionType == PK_DeviceType_NetworkDevice)
	{
        return SendEthRequestBig(device);
		//return SendEthRequest(device);
        //return PK_ERR_TRANSFER;
	}

#ifdef POKEYSLIB_USE_LIBUSB
	if (device->connectionType == PK_DeviceType_FastUSBDevice)
		return SendRequestFastUSB_multiPart(device);
	else
		return PK_ERR_TRANSFER;
#else
    return PK_ERR_TRANSFER;
#endif
}


//#define PK_COM_DEBUG
/**
 * @brief Send a request and wait for the device response.
 *
 * The function finalises the 64&nbsp;byte request by prepending the
 * packet header (0xBB), assigning a request ID and calculating the
 * checksum (simple sum of bytes 0&ndash;6). The request is transmitted
 * over the currently selected interface. After sending the packet the
 * function waits for a reply with header 0xAA that carries the same
 * request ID and a valid checksum.
 *
 * @param device Device that holds the prepared request buffer.
 * @return PK_OK on success or PK_ERR_TRANSFER on failure.
 */
int32_t SendRequest(sPoKeysDevice* device)
{
    // Initialize variables
    uint32_t waits = 0;
    uint32_t retries = 0;
    int32_t result = 0;
    uint8_t bufferOut[65] = {0};

    #ifdef PK_COM_DEBUG
        int i;
    #endif

    hid_device * devHandle;



    if (device == NULL) return PK_ERR_GENERIC;
	if (device->connectionType == PK_DeviceType_NetworkDevice)
	{
		return SendEthRequest(device);
	}
#ifdef POKEYSLIB_USE_LIBUSB
		if (device->connectionType == PK_DeviceType_FastUSBDevice)
			return SendRequestFastUSB(device);
#endif
    devHandle = (hid_device*)device->devHandle;


    if (devHandle == NULL) return PK_ERR_GENERIC;

    // Request sending loop
    while (retries++ < 2)
    {
        device->request[0] = 0xBB;
        device->request[6] = ++device->requestID;
        device->request[7] = getChecksum(device->request);

        memcpy(bufferOut + 1, device->request, 64);

        #ifdef PK_COM_DEBUG
                printf("\n * SEND ");
                for (i = 0; i < 10; i++)
                {
                        printf("%X ", bufferOut[i]);
                }
        #endif
#ifndef RTAPI
        result = hid_write(devHandle, bufferOut, 65);
#endif
        // In case of an error, try sending again
        if (result < 0)
        {
            //printf(" ERR %u", result);
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: SendRequest: ERR %u\n", __FILE__, __FUNCTION__, result);
            retries++;
            continue;
        }

        waits = 0;

        // Request receiving loop
        while (waits++ < 50)
        {
            #ifndef RTAPI
            result = hid_read(devHandle, device->response, 65);
            #endif
            // Error is not an option
            if (result < 0)
            {
                    //printf(" Receive ERR %u", result);
                    rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: Receive ERR %u\n", __FILE__, __FUNCTION__, result);
                    break;
            }

#ifdef PK_COM_DEBUG
        printf("\n * RECV ");
        for (i = 0; i < 10; i++)
        {
                printf("%X ", device->response[i]);
        }

        printf(" (request ID: %X ?= %X", device->response[6], device->requestID);
#endif

                // Check the header and the request ID
                if (device->response[0] == 0xAA && device->response[6] == device->requestID)
                {
                    if (device->response[7] == getChecksum(device->response))
                    {
                        LastRetryCount = retries;
                        LastWaitCount = waits;
                        // This is it. Return from this function
                        return PK_OK;
                    }
                }
            }
    }

    return PK_ERR_TRANSFER;
}

//#define PK_COM_DEBUG
/**
 * @brief Send a request without waiting for any reply.
 *
 * The packet formatting is identical to SendRequest() but the function
 * returns immediately after the HID write call. It is typically used
 * with commands that acknowledge through other means (for example
 * ::PK_CMD_REBOOT_SYSTEM).
 *
 * @param device Device that holds the prepared request buffer.
 * @return PK_OK on successful USB transfer or PK_ERR_TRANSFER when the
 *         packet could not be sent.
 */
int32_t SendRequest_NoResponse(sPoKeysDevice* device)
{
    // Initialize variables
    uint32_t waits = 0;
    uint32_t retries = 0;
    int32_t result = 0;
    uint8_t bufferOut[65] = {0};

    #ifdef PK_COM_DEBUG
        int i;
    #endif

    hid_device * devHandle;



    if (device == NULL) return PK_ERR_GENERIC;
	if (device->connectionType == PK_DeviceType_NetworkDevice)
	{
		return SendEthRequest_NoResponse(device);
	}
#ifdef POKEYSLIB_USE_LIBUSB
		if (device->connectionType == PK_DeviceType_FastUSBDevice)
			return SendRequestFastUSB_NoResponse(device);
#endif
    devHandle = (hid_device*)device->devHandle;


    if (devHandle == NULL) return PK_ERR_GENERIC;

    // Request sending loop
    while (retries++ < 2)
    {
        device->request[0] = 0xBB;
        device->request[6] = ++device->requestID;
        device->request[7] = getChecksum(device->request);

        memcpy(bufferOut + 1, device->request, 64);

        #ifdef PK_COM_DEBUG
                printf("\n * SEND ");
                for (i = 0; i < 10; i++)
                {
                        printf("%X ", bufferOut[i]);
                }
        #endif
#ifndef RTAPI
        result = hid_write(devHandle, bufferOut, 65);
#endif
        // In case of an error, try sending again
        if (result < 0)
        {
            //printf(" ERR %u", result);
            rtapi_print_msg(RTAPI_MSG_ERR, "PoKeys: %s:%s: SendRequest_NoResponse: ERR %u\n", __FILE__, __FUNCTION__, result);
            retries++;
            continue;
        }
    }

    return PK_ERR_TRANSFER;
}



// ******************************************************************************************
// These functions are used to access the device data without using the structures...
// ******************************************************************************************
void PK_SL_SetPinFunction(sPoKeysDevice* device, uint8_t pin, uint8_t function)
{
    device->Pins[pin].PinFunction = function;
}

uint8_t PK_SL_GetPinFunction(sPoKeysDevice* device, uint8_t pin)
{
    return device->Pins[pin].PinFunction;
}

void PK_SL_DigitalOutputSet(sPoKeysDevice* device, uint8_t pin, uint8_t value)
{
    *(device->Pins[pin].DigitalValueSet.out) = value;
}

uint8_t PK_SL_DigitalInputGet(sPoKeysDevice* device, uint8_t pin)
{
    return *(device->Pins[pin].DigitalValueGet.in);
}

uint32_t PK_SL_AnalogInputGet(sPoKeysDevice* device, uint8_t pin)
{
    return *(device->Pins[pin].AnalogValue);
}