[libsigrok.git] / src / hardware / ipdbg-logic-analyser / protocol.c

/*
 * This file is part of the libsigrok project.
 *
 * Copyright (C) 2016 danselmi <da@da>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <config.h>

#ifdef _WIN32
#define _WIN32_WINNT 0x0501
#include <winsock2.h>
#include <ws2tcpip.h>
#endif

#include <string.h>
#include <unistd.h>

#ifndef _WIN32
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#endif

#include <errno.h>
#include "protocol.h"

#include <sys/ioctl.h>


#define BUFFER_SIZE 4

/* Top-level command opcodes */
#define CMD_SET_TRIGGER            0x00
#define CMD_CFG_TRIGGER            0xF0
#define CMD_CFG_LA                 0x0F
#define CMD_START                  0xFE
#define CMD_RESET                  0xEE

#define CMD_GET_BUS_WIDTHS         0xAA
#define CMD_GET_LA_ID              0xBB
#define CMD_ESCAPE                 0x55

/* Trigger subfunction command opcodes */
#define CMD_TRIG_MASKS             0xF1
#define CMD_TRIG_MASK              0xF3
#define CMD_TRIG_VALUE             0xF7

#define CMD_TRIG_MASKS_LAST        0xF9
#define CMD_TRIG_MASK_LAST         0xFB
#define CMD_TRIG_VALUE_LAST        0xFF

#define CMD_TRIG_SELECT_EDGE_MASK  0xF5
#define CMD_TRIG_SET_EDGE_MASK     0xF6

/* LA subfunction command opcodes */
#define CMD_LA_DELAY               0x1F


SR_PRIV int data_available(struct ipdbg_org_la_tcp *tcp)
{
#ifdef __WIN32__
    ioctlsocket(tcp->socket, FIONREAD, &bytes_available);
#else
    int status;

    if (ioctl(tcp->socket, FIONREAD, &status) < 0) {  // TIOCMGET
           sr_err("FIONREAD failed: %s\n", strerror(errno));
            return 0;
    }

    return (status < 1) ? 0 : 1;
#endif  // __WIN32__
}

SR_PRIV struct ipdbg_org_la_tcp *ipdbg_org_la_tcp_new(void)
{
    struct ipdbg_org_la_tcp *tcp;

    tcp = g_malloc0(sizeof(struct ipdbg_org_la_tcp));

    tcp->address = NULL;
    tcp->port = NULL;
    tcp->socket = -1;

    return tcp;
}

SR_PRIV void ipdbg_org_la_tcp_free(struct ipdbg_org_la_tcp *tcp)
{
    g_free(tcp->address);
    g_free(tcp->port);
}


SR_PRIV int ipdbg_org_la_tcp_open(struct ipdbg_org_la_tcp *tcp)
{
    struct addrinfo hints;
    struct addrinfo *results, *res;
    int err;

    memset(&hints, 0, sizeof(hints));
    hints.ai_family = AF_UNSPEC;
    hints.ai_socktype = SOCK_STREAM;
    hints.ai_protocol = IPPROTO_TCP;

    err = getaddrinfo(tcp->address, tcp->port, &hints, &results);

    if (err) {
        sr_err("Address lookup failed: %s:%s: %s", tcp->address, tcp->port,
            gai_strerror(err));
        return SR_ERR;
    }

    for (res = results; res; res = res->ai_next) {
        if ((tcp->socket = socket(res->ai_family, res->ai_socktype,
                        res->ai_protocol)) < 0)
            continue;
        if (connect(tcp->socket, res->ai_addr, res->ai_addrlen) != 0) {
            close(tcp->socket);
            tcp->socket = -1;
            continue;
        }
        break;
    }

    freeaddrinfo(results);

    if (tcp->socket < 0) {
        sr_err("Failed to connect to %s:%s: %s", tcp->address, tcp->port,
                g_strerror(errno));
        return SR_ERR;
    }

    return SR_OK;
}

SR_PRIV int ipdbg_org_la_tcp_close(struct ipdbg_org_la_tcp *tcp)
{
    int ret = SR_OK;

    if (close(tcp->socket) < 0)
        ret = SR_ERR;

    tcp->socket = -1;

    return ret;
}

SR_PRIV int ipdbg_org_la_tcp_send(struct ipdbg_org_la_tcp *tcp,
	const uint8_t *buf, size_t len)
{
    int out;
    out = send(tcp->socket, (char*)buf, len, 0);

    if (out < 0) {
        sr_err("Send error: %s", g_strerror(errno));
        return SR_ERR;
    }

    if (out < (int)len)
        sr_dbg("Only sent %d/%d bytes of data.", out, (int)len);

    return SR_OK;
}

SR_PRIV int ipdbg_org_la_tcp_receive_blocking(struct ipdbg_org_la_tcp *tcp,
	uint8_t *buf, int bufsize)
{
    int received = 0;

    while (received < bufsize) {
        if (ipdbg_org_la_tcp_receive(tcp, buf) > 0) {
            buf++;
            received++;
        }
    }

    return received;
}

SR_PRIV int ipdbg_org_la_tcp_receive(struct ipdbg_org_la_tcp *tcp, uint8_t *buf)
{
    int received = 0;

    if (data_available(tcp)) {
        while (received < 1) {
            int len = recv(tcp->socket, buf, 1, 0);

            if (len < 0) {
                sr_err("Receive error: %s", g_strerror(errno));
                return SR_ERR;
            } else
            	received += len;
        }

        return received;
    }
    else
        return -1;
}

SR_PRIV int ipdbg_org_la_convert_trigger(const struct sr_dev_inst *sdi)
{
    struct ipdbg_org_la_dev_context *devc;
    struct sr_trigger *trigger;
    struct sr_trigger_stage *stage;
    struct sr_trigger_match *match;
    const GSList *l, *m;

    devc = sdi->priv;

    devc->num_stages = 0;
    devc->num_transfers = 0;
    devc->raw_sample_buf = NULL;

    for (uint64_t i = 0; i < devc->DATA_WIDTH_BYTES; i++) {
        devc->trigger_mask[i] = 0;
        devc->trigger_value[i] = 0;
        devc->trigger_mask_last[i] = 0;
        devc->trigger_value_last[i] = 0;
        devc->trigger_edge_mask[i] = 0;
    }

    if (!(trigger = sr_session_trigger_get(sdi->session)))
        return SR_OK;

    for (l = trigger->stages; l; l = l->next) {
        stage = l->data;
        for (m = stage->matches; m; m = m->next) {
            match = m->data;
            int byte_idx = match->channel->index / 8;
            uint8_t match_bit = 1 << (match->channel->index % 8);

            if (!match->channel->enabled)
                /* Ignore disabled channels with a trigger. */
                continue;

            if (match->match == SR_TRIGGER_ONE) {
                devc->trigger_value[byte_idx] |= match_bit;
                devc->trigger_mask[byte_idx] |= match_bit;
                devc->trigger_mask_last[byte_idx] &= ~match_bit;
                devc->trigger_edge_mask[byte_idx] &= ~match_bit;
            } else if (match->match == SR_TRIGGER_ZERO) {
                devc->trigger_value[byte_idx] &= ~match_bit;
                devc->trigger_mask[byte_idx] |= match_bit;
                devc->trigger_mask_last[byte_idx] &= ~match_bit;
                devc->trigger_edge_mask[byte_idx] &= ~match_bit;
            } else if ( match->match == SR_TRIGGER_RISING) {
                devc->trigger_value[byte_idx] |= match_bit;
                devc->trigger_value_last[byte_idx] &= ~match_bit;
                devc->trigger_mask[byte_idx] |= match_bit;
                devc->trigger_mask_last[byte_idx] |= match_bit;
                devc->trigger_edge_mask[byte_idx] &= ~match_bit;
            } else if (match->match == SR_TRIGGER_FALLING) {
                devc->trigger_value[byte_idx] &= ~match_bit;
                devc->trigger_value_last[byte_idx] |= match_bit;
                devc->trigger_mask[byte_idx] |= match_bit;
                devc->trigger_mask_last[byte_idx] |= match_bit;
                devc->trigger_edge_mask[byte_idx] &= ~match_bit;
            } else if (match->match == SR_TRIGGER_EDGE) {
                devc->trigger_mask[byte_idx] &= ~match_bit;
                devc->trigger_mask_last[byte_idx] &= ~match_bit;
                devc->trigger_edge_mask[byte_idx] |= match_bit;
            }
        }
    }

    return SR_OK;
}

SR_PRIV int ipdbg_org_la_receive_data(int fd, int revents, void *cb_data)
{
    const struct sr_dev_inst *sdi;
    struct ipdbg_org_la_dev_context *devc;

    (void)fd;
    (void)revents;

    sdi = (const struct sr_dev_inst*)cb_data;
    if (!sdi)
        return FALSE;

    if (!(devc = sdi->priv))
        return FALSE;

    struct ipdbg_org_la_tcp *tcp = sdi->conn;
    struct sr_datafeed_packet packet;
    struct sr_datafeed_logic logic;

    if (!devc->raw_sample_buf) {
        devc->raw_sample_buf =
        	g_try_malloc(devc->limit_samples * devc->DATA_WIDTH_BYTES);
        if (!devc->raw_sample_buf) {
            sr_warn("Sample buffer malloc failed.");
            return FALSE;
        }
    }

    if (devc->num_transfers < (devc->limit_samples_max * devc->DATA_WIDTH_BYTES)) {
        uint8_t byte;

        if (ipdbg_org_la_tcp_receive(tcp, &byte) == 1) {
            if (devc->num_transfers < (devc->limit_samples * devc->DATA_WIDTH_BYTES))
                devc->raw_sample_buf[devc->num_transfers] = byte;

            devc->num_transfers++;
        }
    } else {
        if (devc->delay_value > 0) {
            /* There are pre-trigger samples, send those first. */
            packet.type = SR_DF_LOGIC;
            packet.payload = &logic;
            logic.length = devc->delay_value * devc->DATA_WIDTH_BYTES;
            logic.unitsize = devc->DATA_WIDTH_BYTES;
            logic.data = devc->raw_sample_buf;
            sr_session_send(cb_data, &packet);
        }

        /* Send the trigger. */
        packet.type = SR_DF_TRIGGER;
        sr_session_send(cb_data, &packet);

        /* Send post-trigger samples. */
        packet.type = SR_DF_LOGIC;
        packet.payload = &logic;
        logic.length = (devc->limit_samples - devc->delay_value) * devc->DATA_WIDTH_BYTES;
        logic.unitsize = devc->DATA_WIDTH_BYTES;
        logic.data = devc->raw_sample_buf + (devc->delay_value * devc->DATA_WIDTH_BYTES);
        sr_session_send(cb_data, &packet);

        g_free(devc->raw_sample_buf);
        devc->raw_sample_buf = NULL;

        ipdbg_org_la_abort_acquisition(sdi);
    }

    return TRUE;
}

SR_PRIV int ipdbg_org_la_send_delay(struct ipdbg_org_la_dev_context *devc,
	struct ipdbg_org_la_tcp *tcp)
{
    devc->delay_value = (devc->limit_samples / 100.0) * devc->capture_ratio;

    uint8_t buf;
    buf = CMD_CFG_LA;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_LA_DELAY;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    uint8_t delay_buf[4] = { devc->delay_value        & 0x000000ff,
                            (devc->delay_value >>  8) & 0x000000ff,
                            (devc->delay_value >> 16) & 0x000000ff,
                            (devc->delay_value >> 24) & 0x000000ff};

    for (uint64_t i = 0; i < devc->ADDR_WIDTH_BYTES; i++)
        send_escaping(tcp, &(delay_buf[devc->ADDR_WIDTH_BYTES-1-i]), 1);

    return SR_OK;
}

SR_PRIV int ipdbg_org_la_send_trigger(struct ipdbg_org_la_dev_context *devc,
	struct ipdbg_org_la_tcp *tcp)
{
    uint8_t buf;

    /* Mask */
    buf = CMD_CFG_TRIGGER;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASKS;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASK;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
        send_escaping(tcp, devc->trigger_mask + devc->DATA_WIDTH_BYTES - 1 - i, 1);

    /* Value */
    buf = CMD_CFG_TRIGGER;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASKS;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_VALUE;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
        send_escaping(tcp, devc->trigger_value + devc->DATA_WIDTH_BYTES - 1 - i, 1);

    /* Mask_last */
    buf = CMD_CFG_TRIGGER;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASKS_LAST;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASK_LAST;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
        send_escaping(tcp, devc->trigger_mask_last + devc->DATA_WIDTH_BYTES - 1 - i, 1);

    /* Value_last */
    buf = CMD_CFG_TRIGGER;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_MASKS_LAST;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_VALUE_LAST;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
        send_escaping(tcp, devc->trigger_value_last + devc->DATA_WIDTH_BYTES - 1 - i, 1);

    /* Edge_mask */
    buf = CMD_CFG_TRIGGER;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_SELECT_EDGE_MASK;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);
    buf = CMD_TRIG_SET_EDGE_MASK;
    ipdbg_org_la_tcp_send(tcp, &buf, 1);

    for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
        send_escaping(tcp, devc->trigger_edge_mask + devc->DATA_WIDTH_BYTES - 1 - i, 1);

    return SR_OK;
}

SR_PRIV int send_escaping(struct ipdbg_org_la_tcp *tcp, uint8_t *dataToSend, uint32_t length)
{
    uint8_t escape = CMD_ESCAPE;

    while (length--) {
        uint8_t payload = *dataToSend++;

        if (payload == (uint8_t)CMD_RESET)
            if (ipdbg_org_la_tcp_send(tcp, &escape, 1) != SR_OK)
                sr_warn("Couldn't send escape");

        if (payload == (uint8_t)CMD_ESCAPE)
            if (ipdbg_org_la_tcp_send(tcp, &escape, 1) != SR_OK)
                sr_warn("Couldn't send escape");

        if (ipdbg_org_la_tcp_send(tcp, &payload, 1) != SR_OK)
            sr_warn("Couldn't send data");
    }

    return SR_OK;
}

SR_PRIV void ipdbg_org_la_get_addrwidth_and_datawidth(struct ipdbg_org_la_tcp *tcp,
	struct ipdbg_org_la_dev_context *devc)
{
    uint8_t buf[8];
    uint8_t read_cmd = CMD_GET_BUS_WIDTHS;

    if (ipdbg_org_la_tcp_send(tcp, &read_cmd, 1) != SR_OK)
        sr_warn("Can't send read command");

    if (ipdbg_org_la_tcp_receive_blocking(tcp, buf, 8) != 8)
        sr_warn("Can't get address and data width from device");

    devc->DATA_WIDTH  =  buf[0]        & 0x000000FF;
    devc->DATA_WIDTH |= (buf[1] <<  8) & 0x0000FF00;
    devc->DATA_WIDTH |= (buf[2] << 16) & 0x00FF0000;
    devc->DATA_WIDTH |= (buf[3] << 24) & 0xFF000000;

    devc->ADDR_WIDTH  =  buf[4]        & 0x000000FF;
    devc->ADDR_WIDTH |= (buf[5] <<  8) & 0x0000FF00;
    devc->ADDR_WIDTH |= (buf[6] << 16) & 0x00FF0000;
    devc->ADDR_WIDTH |= (buf[7] << 24) & 0xFF000000;

    uint8_t HOST_WORD_SIZE = 8;

    devc->DATA_WIDTH_BYTES = (devc->DATA_WIDTH + HOST_WORD_SIZE - 1) / HOST_WORD_SIZE;
    devc->ADDR_WIDTH_BYTES = (devc->ADDR_WIDTH + HOST_WORD_SIZE - 1) / HOST_WORD_SIZE;
    devc->limit_samples_max = (0x01 << devc->ADDR_WIDTH);
    devc->limit_samples = devc->limit_samples_max;

    devc->trigger_mask       = g_malloc0(devc->DATA_WIDTH_BYTES);
    devc->trigger_value      = g_malloc0(devc->DATA_WIDTH_BYTES);
    devc->trigger_mask_last  = g_malloc0(devc->DATA_WIDTH_BYTES);
    devc->trigger_value_last = g_malloc0(devc->DATA_WIDTH_BYTES);
    devc->trigger_edge_mask  = g_malloc0(devc->DATA_WIDTH_BYTES);
}

SR_PRIV struct ipdbg_org_la_dev_context *ipdbg_org_la_dev_new(void)
{
    struct ipdbg_org_la_dev_context *devc;

    devc = g_malloc0(sizeof(struct ipdbg_org_la_dev_context));
    devc->capture_ratio = 50;

    return devc;
}

SR_PRIV int ipdbg_org_la_send_reset(struct ipdbg_org_la_tcp *tcp)
{
    uint8_t buf = CMD_RESET;
    if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
        sr_warn("Couldn't send reset");

    return SR_OK;
}

SR_PRIV int ipdbg_org_la_request_id(struct ipdbg_org_la_tcp *tcp)
{
    uint8_t buf = CMD_GET_LA_ID;
    if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
        sr_warn("Couldn't send ID request");

    char id[4];
    if (ipdbg_org_la_tcp_receive_blocking(tcp, (uint8_t*)id, 4) != 4)
        sr_warn("Couldn't read device ID");

    if (strncmp(id, "IDBG", 4)) {
        sr_err("Invalid device ID: expected 'IDBG', got '%c%c%c%c'.",
        	id[0], id[1], id[2], id[3]);
        return SR_ERR;
    }

    return SR_OK;
}

SR_PRIV void ipdbg_org_la_abort_acquisition(const struct sr_dev_inst *sdi)
{
    struct ipdbg_org_la_tcp *tcp = sdi->conn;

    sr_session_source_remove(sdi->session, tcp->socket);

    std_session_send_df_end(sdi);
}

SR_PRIV int ipdbg_org_la_send_start(struct ipdbg_org_la_tcp *tcp)
{
    uint8_t buf = CMD_START;

   if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
        sr_warn("Couldn't send start");

   return SR_OK;
}
Commit	Line	Data
a4210e18 EK	1	/*
	2	* This file is part of the libsigrok project.
	3	*
	4	* Copyright (C) 2016 danselmi <da@da>
	5	*
	6	* This program is free software: you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation, either version 3 of the License, or
	9	* (at your option) any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU General Public License
	17	* along with this program. If not, see <http://www.gnu.org/licenses/>.
	18	*/
	19
	20	#include <config.h>
7f4c9a04	21
a4210e18 EK	22	#ifdef _WIN32
	23	#define _WIN32_WINNT 0x0501
	24	#include <winsock2.h>
	25	#include <ws2tcpip.h>
	26	#endif
7f4c9a04	27
a4210e18 EK	28	#include <string.h>
a4210e18 EK	29	#include <unistd.h>
7f4c9a04	30
a4210e18 EK	31	#ifndef _WIN32
	32	#include <sys/socket.h>
	33	#include <netinet/in.h>
	34	#include <arpa/inet.h>
	35	#include <netdb.h>
	36	#endif
7f4c9a04	37
a4210e18 EK	38	#include <errno.h>
	39	#include "protocol.h"
	40
b51288e3 EK	41	#include <sys/ioctl.h>
b51288e3 EK	42
a4210e18	43
7f4c9a04	44	#define BUFFER_SIZE 4
2f15f5bf	45
7f4c9a04 SA	46	/* Top-level command opcodes */
	47	#define CMD_SET_TRIGGER 0x00
	48	#define CMD_CFG_TRIGGER 0xF0
	49	#define CMD_CFG_LA 0x0F
	50	#define CMD_START 0xFE
	51	#define CMD_RESET 0xEE
2f15f5bf	52
7f4c9a04 SA	53	#define CMD_GET_BUS_WIDTHS 0xAA
	54	#define CMD_GET_LA_ID 0xBB
	55	#define CMD_ESCAPE 0x55
2f15f5bf	56
7f4c9a04 SA	57	/* Trigger subfunction command opcodes */
	58	#define CMD_TRIG_MASKS 0xF1
	59	#define CMD_TRIG_MASK 0xF3
	60	#define CMD_TRIG_VALUE 0xF7
2f15f5bf	61
7f4c9a04 SA	62	#define CMD_TRIG_MASKS_LAST 0xF9
	63	#define CMD_TRIG_MASK_LAST 0xFB
	64	#define CMD_TRIG_VALUE_LAST 0xFF
2f15f5bf	65
7f4c9a04 SA	66	#define CMD_TRIG_SELECT_EDGE_MASK 0xF5
7f4c9a04 SA	67	#define CMD_TRIG_SET_EDGE_MASK 0xF6
2f15f5bf	68
7f4c9a04 SA	69	/* LA subfunction command opcodes */
7f4c9a04 SA	70	#define CMD_LA_DELAY 0x1F
a66099c6	71
a4210e18	72
7f4c9a04	73	SR_PRIV int data_available(struct ipdbg_org_la_tcp *tcp)
b51288e3 EK	74	{
b51288e3 EK	75	#ifdef __WIN32__
7f4c9a04	76	ioctlsocket(tcp->socket, FIONREAD, &bytes_available);
b51288e3	77	#else
b51288e3 EK	78	int status;
b51288e3 EK	79
7f4c9a04 SA	80	if (ioctl(tcp->socket, FIONREAD, &status) < 0) { // TIOCMGET
7f4c9a04 SA	81	sr_err("FIONREAD failed: %s\n", strerror(errno));
b51288e3	82	return 0;
b51288e3 EK	83	}
b51288e3 EK	84
7f4c9a04 SA	85	return (status < 1) ? 0 : 1;
7f4c9a04 SA	86	#endif // __WIN32__
b51288e3 EK	87	}
b51288e3 EK	88
7f4c9a04	89	SR_PRIV struct ipdbg_org_la_tcp *ipdbg_org_la_tcp_new(void)
a4210e18 EK	90	{
	91	struct ipdbg_org_la_tcp *tcp;
	92
	93	tcp = g_malloc0(sizeof(struct ipdbg_org_la_tcp));
	94
	95	tcp->address = NULL;
	96	tcp->port = NULL;
	97	tcp->socket = -1;
	98
	99	return tcp;
	100	}
	101
7f4c9a04 SA	102	SR_PRIV void ipdbg_org_la_tcp_free(struct ipdbg_org_la_tcp *tcp)
	103	{
	104	g_free(tcp->address);
	105	g_free(tcp->port);
	106	}
	107
	108
a4210e18 EK	109	SR_PRIV int ipdbg_org_la_tcp_open(struct ipdbg_org_la_tcp *tcp)
a4210e18 EK	110	{
2f15f5bf EK	111	struct addrinfo hints;
	112	struct addrinfo results, res;
	113	int err;
	114
	115	memset(&hints, 0, sizeof(hints));
	116	hints.ai_family = AF_UNSPEC;
	117	hints.ai_socktype = SOCK_STREAM;
	118	hints.ai_protocol = IPPROTO_TCP;
	119
	120	err = getaddrinfo(tcp->address, tcp->port, &hints, &results);
	121
	122	if (err) {
	123	sr_err("Address lookup failed: %s:%s: %s", tcp->address, tcp->port,
	124	gai_strerror(err));
	125	return SR_ERR;
	126	}
	127
	128	for (res = results; res; res = res->ai_next) {
	129	if ((tcp->socket = socket(res->ai_family, res->ai_socktype,
	130	res->ai_protocol)) < 0)
	131	continue;
	132	if (connect(tcp->socket, res->ai_addr, res->ai_addrlen) != 0) {
	133	close(tcp->socket);
	134	tcp->socket = -1;
	135	continue;
	136	}
	137	break;
	138	}
	139
	140	freeaddrinfo(results);
	141
	142	if (tcp->socket < 0) {
	143	sr_err("Failed to connect to %s:%s: %s", tcp->address, tcp->port,
	144	g_strerror(errno));
	145	return SR_ERR;
	146	}
	147
	148	return SR_OK;
a4210e18 EK	149	}
a4210e18 EK	150
7f4c9a04 SA	151	SR_PRIV int ipdbg_org_la_tcp_close(struct ipdbg_org_la_tcp *tcp)
	152	{
	153	int ret = SR_OK;
	154
	155	if (close(tcp->socket) < 0)
	156	ret = SR_ERR;
	157
	158	tcp->socket = -1;
	159
	160	return ret;
	161	}
	162
	163	SR_PRIV int ipdbg_org_la_tcp_send(struct ipdbg_org_la_tcp *tcp,
	164	const uint8_t *buf, size_t len)
a4210e18	165	{
2f15f5bf EK	166	int out;
2f15f5bf EK	167	out = send(tcp->socket, (char*)buf, len, 0);
a4210e18	168
2f15f5bf EK	169	if (out < 0) {
	170	sr_err("Send error: %s", g_strerror(errno));
	171	return SR_ERR;
	172	}
a4210e18	173
7f4c9a04	174	if (out < (int)len)
2f15f5bf	175	sr_dbg("Only sent %d/%d bytes of data.", out, (int)len);
a4210e18	176
2f15f5bf	177	return SR_OK;
a4210e18 EK	178	}
a4210e18 EK	179
7f4c9a04 SA	180	SR_PRIV int ipdbg_org_la_tcp_receive_blocking(struct ipdbg_org_la_tcp *tcp,
7f4c9a04 SA	181	uint8_t *buf, int bufsize)
a4210e18 EK	182	{
a4210e18 EK	183	int received = 0;
7f4c9a04 SA	184
	185	while (received < bufsize) {
	186	if (ipdbg_org_la_tcp_receive(tcp, buf) > 0) {
	187	buf++;
	188	received++;
a4210e18 EK	189	}
a4210e18 EK	190	}
7f4c9a04	191
2f15f5bf	192	return received;
a4210e18	193	}
7f4c9a04	194
b51288e3 EK	195	SR_PRIV int ipdbg_org_la_tcp_receive(struct ipdbg_org_la_tcp tcp, uint8_t buf)
	196	{
	197	int received = 0;
	198
7f4c9a04 SA	199	if (data_available(tcp)) {
7f4c9a04 SA	200	while (received < 1) {
b51288e3 EK	201	int len = recv(tcp->socket, buf, 1, 0);
b51288e3 EK	202
7f4c9a04	203	if (len < 0) {
b51288e3 EK	204	sr_err("Receive error: %s", g_strerror(errno));
b51288e3 EK	205	return SR_ERR;
7f4c9a04 SA	206	} else
7f4c9a04 SA	207	received += len;
b51288e3	208	}
b51288e3	209
7f4c9a04	210	return received;
b51288e3 EK	211	}
b51288e3 EK	212	else
b51288e3	213	return -1;
a4210e18 EK	214	}
	215
	216	SR_PRIV int ipdbg_org_la_convert_trigger(const struct sr_dev_inst *sdi)
	217	{
	218	struct ipdbg_org_la_dev_context *devc;
	219	struct sr_trigger *trigger;
	220	struct sr_trigger_stage *stage;
	221	struct sr_trigger_match *match;
	222	const GSList l, m;
	223
	224	devc = sdi->priv;
	225
	226	devc->num_stages = 0;
	227	devc->num_transfers = 0;
7f4c9a04 SA	228	devc->raw_sample_buf = NULL;
	229
	230	for (uint64_t i = 0; i < devc->DATA_WIDTH_BYTES; i++) {
a4210e18 EK	231	devc->trigger_mask[i] = 0;
	232	devc->trigger_value[i] = 0;
	233	devc->trigger_mask_last[i] = 0;
	234	devc->trigger_value_last[i] = 0;
a66099c6	235	devc->trigger_edge_mask[i] = 0;
a4210e18	236	}
a4210e18	237
b8fa29a1	238	if (!(trigger = sr_session_trigger_get(sdi->session)))
a4210e18	239	return SR_OK;
a4210e18	240
7f4c9a04 SA	241	for (l = trigger->stages; l; l = l->next) {
	242	stage = l->data;
	243	for (m = stage->matches; m; m = m->next) {
a4210e18	244	match = m->data;
7f4c9a04 SA	245	int byte_idx = match->channel->index / 8;
7f4c9a04 SA	246	uint8_t match_bit = 1 << (match->channel->index % 8);
a4210e18 EK	247
	248	if (!match->channel->enabled)
	249	/* Ignore disabled channels with a trigger. */
	250	continue;
a4210e18	251
7f4c9a04 SA	252	if (match->match == SR_TRIGGER_ONE) {
	253	devc->trigger_value[byte_idx] \|= match_bit;
	254	devc->trigger_mask[byte_idx] \|= match_bit;
	255	devc->trigger_mask_last[byte_idx] &= ~match_bit;
	256	devc->trigger_edge_mask[byte_idx] &= ~match_bit;
	257	} else if (match->match == SR_TRIGGER_ZERO) {
	258	devc->trigger_value[byte_idx] &= ~match_bit;
	259	devc->trigger_mask[byte_idx] \|= match_bit;
	260	devc->trigger_mask_last[byte_idx] &= ~match_bit;
	261	devc->trigger_edge_mask[byte_idx] &= ~match_bit;
	262	} else if ( match->match == SR_TRIGGER_RISING) {
	263	devc->trigger_value[byte_idx] \|= match_bit;
	264	devc->trigger_value_last[byte_idx] &= ~match_bit;
	265	devc->trigger_mask[byte_idx] \|= match_bit;
	266	devc->trigger_mask_last[byte_idx] \|= match_bit;
	267	devc->trigger_edge_mask[byte_idx] &= ~match_bit;
	268	} else if (match->match == SR_TRIGGER_FALLING) {
	269	devc->trigger_value[byte_idx] &= ~match_bit;
	270	devc->trigger_value_last[byte_idx] \|= match_bit;
	271	devc->trigger_mask[byte_idx] \|= match_bit;
	272	devc->trigger_mask_last[byte_idx] \|= match_bit;
	273	devc->trigger_edge_mask[byte_idx] &= ~match_bit;
	274	} else if (match->match == SR_TRIGGER_EDGE) {
	275	devc->trigger_mask[byte_idx] &= ~match_bit;
	276	devc->trigger_mask_last[byte_idx] &= ~match_bit;
	277	devc->trigger_edge_mask[byte_idx] \|= match_bit;
a4210e18	278	}
a4210e18	279	}
a4210e18 EK	280	}
a4210e18 EK	281
a4210e18 EK	282	return SR_OK;
	283	}
	284
	285	SR_PRIV int ipdbg_org_la_receive_data(int fd, int revents, void *cb_data)
	286	{
a4210e18 EK	287	const struct sr_dev_inst *sdi;
	288	struct ipdbg_org_la_dev_context *devc;
	289
a4210e18	290	(void)fd;
2f15f5bf	291	(void)revents;
a4210e18	292
7f4c9a04	293	sdi = (const struct sr_dev_inst*)cb_data;
a4210e18	294	if (!sdi)
a4210e18	295	return FALSE;
a4210e18 EK	296
a4210e18 EK	297	if (!(devc = sdi->priv))
a4210e18 EK	298	return FALSE;
a4210e18 EK	299
a4210e18 EK	300	struct ipdbg_org_la_tcp *tcp = sdi->conn;
	301	struct sr_datafeed_packet packet;
	302	struct sr_datafeed_logic logic;
	303
7f4c9a04 SA	304	if (!devc->raw_sample_buf) {
	305	devc->raw_sample_buf =
	306	g_try_malloc(devc->limit_samples * devc->DATA_WIDTH_BYTES);
a4210e18 EK	307	if (!devc->raw_sample_buf) {
	308	sr_warn("Sample buffer malloc failed.");
	309	return FALSE;
	310	}
a4210e18 EK	311	}
a4210e18 EK	312
7f4c9a04 SA	313	if (devc->num_transfers < (devc->limit_samples_max * devc->DATA_WIDTH_BYTES)) {
7f4c9a04 SA	314	uint8_t byte;
a4210e18	315
7f4c9a04 SA	316	if (ipdbg_org_la_tcp_receive(tcp, &byte) == 1) {
7f4c9a04 SA	317	if (devc->num_transfers < (devc->limit_samples * devc->DATA_WIDTH_BYTES))
a4210e18	318	devc->raw_sample_buf[devc->num_transfers] = byte;
b8fa29a1	319
a4210e18 EK	320	devc->num_transfers++;
a4210e18 EK	321	}
7f4c9a04	322	} else {
a4210e18 EK	323	if (devc->delay_value > 0) {
	324	/* There are pre-trigger samples, send those first. */
	325	packet.type = SR_DF_LOGIC;
	326	packet.payload = &logic;
7f4c9a04	327	logic.length = devc->delay_value * devc->DATA_WIDTH_BYTES;
a4210e18 EK	328	logic.unitsize = devc->DATA_WIDTH_BYTES;
	329	logic.data = devc->raw_sample_buf;
	330	sr_session_send(cb_data, &packet);
	331	}
	332
	333	/* Send the trigger. */
	334	packet.type = SR_DF_TRIGGER;
	335	sr_session_send(cb_data, &packet);
	336
	337	/* Send post-trigger samples. */
	338	packet.type = SR_DF_LOGIC;
	339	packet.payload = &logic;
7f4c9a04	340	logic.length = (devc->limit_samples - devc->delay_value) * devc->DATA_WIDTH_BYTES;
a4210e18	341	logic.unitsize = devc->DATA_WIDTH_BYTES;
7f4c9a04	342	logic.data = devc->raw_sample_buf + (devc->delay_value * devc->DATA_WIDTH_BYTES);
a4210e18 EK	343	sr_session_send(cb_data, &packet);
	344
	345	g_free(devc->raw_sample_buf);
	346	devc->raw_sample_buf = NULL;
	347
b8fa29a1	348	ipdbg_org_la_abort_acquisition(sdi);
a4210e18 EK	349	}
	350
	351	return TRUE;
	352	}
	353
7f4c9a04 SA	354	SR_PRIV int ipdbg_org_la_send_delay(struct ipdbg_org_la_dev_context *devc,
7f4c9a04 SA	355	struct ipdbg_org_la_tcp *tcp)
a4210e18	356	{
7f4c9a04	357	devc->delay_value = (devc->limit_samples / 100.0) * devc->capture_ratio;
a4210e18	358
7f4c9a04 SA	359	uint8_t buf;
	360	buf = CMD_CFG_LA;
	361	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	362	buf = CMD_LA_DELAY;
	363	ipdbg_org_la_tcp_send(tcp, &buf, 1);
a4210e18	364
7f4c9a04 SA	365	uint8_t delay_buf[4] = { devc->delay_value & 0x000000ff,
	366	(devc->delay_value >> 8) & 0x000000ff,
	367	(devc->delay_value >> 16) & 0x000000ff,
	368	(devc->delay_value >> 24) & 0x000000ff};
a4210e18	369
7f4c9a04 SA	370	for (uint64_t i = 0; i < devc->ADDR_WIDTH_BYTES; i++)
7f4c9a04 SA	371	send_escaping(tcp, &(delay_buf[devc->ADDR_WIDTH_BYTES-1-i]), 1);
a4210e18 EK	372
	373	return SR_OK;
	374	}
	375
7f4c9a04 SA	376	SR_PRIV int ipdbg_org_la_send_trigger(struct ipdbg_org_la_dev_context *devc,
7f4c9a04 SA	377	struct ipdbg_org_la_tcp *tcp)
a4210e18	378	{
7f4c9a04 SA	379	uint8_t buf;
	380
	381	/* Mask */
	382	buf = CMD_CFG_TRIGGER;
	383	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	384	buf = CMD_TRIG_MASKS;
	385	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	386	buf = CMD_TRIG_MASK;
	387	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	388
	389	for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
	390	send_escaping(tcp, devc->trigger_mask + devc->DATA_WIDTH_BYTES - 1 - i, 1);
	391
	392	/* Value */
	393	buf = CMD_CFG_TRIGGER;
	394	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	395	buf = CMD_TRIG_MASKS;
	396	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	397	buf = CMD_TRIG_VALUE;
	398	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	399
	400	for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
	401	send_escaping(tcp, devc->trigger_value + devc->DATA_WIDTH_BYTES - 1 - i, 1);
	402
	403	/* Mask_last */
	404	buf = CMD_CFG_TRIGGER;
	405	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	406	buf = CMD_TRIG_MASKS_LAST;
	407	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	408	buf = CMD_TRIG_MASK_LAST;
	409	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	410
	411	for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
	412	send_escaping(tcp, devc->trigger_mask_last + devc->DATA_WIDTH_BYTES - 1 - i, 1);
	413
	414	/* Value_last */
	415	buf = CMD_CFG_TRIGGER;
	416	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	417	buf = CMD_TRIG_MASKS_LAST;
	418	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	419	buf = CMD_TRIG_VALUE_LAST;
	420	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	421
	422	for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
	423	send_escaping(tcp, devc->trigger_value_last + devc->DATA_WIDTH_BYTES - 1 - i, 1);
	424
	425	/* Edge_mask */
	426	buf = CMD_CFG_TRIGGER;
	427	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	428	buf = CMD_TRIG_SELECT_EDGE_MASK;
	429	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	430	buf = CMD_TRIG_SET_EDGE_MASK;
	431	ipdbg_org_la_tcp_send(tcp, &buf, 1);
	432
	433	for (size_t i = 0; i < devc->DATA_WIDTH_BYTES; i++)
	434	send_escaping(tcp, devc->trigger_edge_mask + devc->DATA_WIDTH_BYTES - 1 - i, 1);
b8fa29a1	435
a4210e18 EK	436	return SR_OK;
	437	}
	438
7f4c9a04	439	SR_PRIV int send_escaping(struct ipdbg_org_la_tcp tcp, uint8_t dataToSend, uint32_t length)
a4210e18	440	{
7f4c9a04	441	uint8_t escape = CMD_ESCAPE;
a4210e18	442
7f4c9a04 SA	443	while (length--) {
7f4c9a04 SA	444	uint8_t payload = *dataToSend++;
a4210e18	445
7f4c9a04 SA	446	if (payload == (uint8_t)CMD_RESET)
	447	if (ipdbg_org_la_tcp_send(tcp, &escape, 1) != SR_OK)
	448	sr_warn("Couldn't send escape");
a4210e18	449
7f4c9a04 SA	450	if (payload == (uint8_t)CMD_ESCAPE)
	451	if (ipdbg_org_la_tcp_send(tcp, &escape, 1) != SR_OK)
	452	sr_warn("Couldn't send escape");
a4210e18 EK	453
a4210e18 EK	454	if (ipdbg_org_la_tcp_send(tcp, &payload, 1) != SR_OK)
7f4c9a04	455	sr_warn("Couldn't send data");
a4210e18	456	}
7f4c9a04	457
a4210e18 EK	458	return SR_OK;
	459	}
	460
7f4c9a04 SA	461	SR_PRIV void ipdbg_org_la_get_addrwidth_and_datawidth(struct ipdbg_org_la_tcp *tcp,
7f4c9a04 SA	462	struct ipdbg_org_la_dev_context *devc)
a4210e18	463	{
a4210e18	464	uint8_t buf[8];
7f4c9a04	465	uint8_t read_cmd = CMD_GET_BUS_WIDTHS;
a4210e18	466
7f4c9a04 SA	467	if (ipdbg_org_la_tcp_send(tcp, &read_cmd, 1) != SR_OK)
7f4c9a04 SA	468	sr_warn("Can't send read command");
a4210e18	469
7f4c9a04 SA	470	if (ipdbg_org_la_tcp_receive_blocking(tcp, buf, 8) != 8)
7f4c9a04 SA	471	sr_warn("Can't get address and data width from device");
b51288e3	472
a4210e18 EK	473	devc->DATA_WIDTH = buf[0] & 0x000000FF;
	474	devc->DATA_WIDTH \|= (buf[1] << 8) & 0x0000FF00;
	475	devc->DATA_WIDTH \|= (buf[2] << 16) & 0x00FF0000;
	476	devc->DATA_WIDTH \|= (buf[3] << 24) & 0xFF000000;
	477
	478	devc->ADDR_WIDTH = buf[4] & 0x000000FF;
	479	devc->ADDR_WIDTH \|= (buf[5] << 8) & 0x0000FF00;
	480	devc->ADDR_WIDTH \|= (buf[6] << 16) & 0x00FF0000;
	481	devc->ADDR_WIDTH \|= (buf[7] << 24) & 0xFF000000;
	482
7f4c9a04	483	uint8_t HOST_WORD_SIZE = 8;
a4210e18	484
7f4c9a04 SA	485	devc->DATA_WIDTH_BYTES = (devc->DATA_WIDTH + HOST_WORD_SIZE - 1) / HOST_WORD_SIZE;
7f4c9a04 SA	486	devc->ADDR_WIDTH_BYTES = (devc->ADDR_WIDTH + HOST_WORD_SIZE - 1) / HOST_WORD_SIZE;
a4210e18	487	devc->limit_samples_max = (0x01 << devc->ADDR_WIDTH);
b8fa29a1	488	devc->limit_samples = devc->limit_samples_max;
a4210e18	489
a4210e18 EK	490	devc->trigger_mask = g_malloc0(devc->DATA_WIDTH_BYTES);
	491	devc->trigger_value = g_malloc0(devc->DATA_WIDTH_BYTES);
	492	devc->trigger_mask_last = g_malloc0(devc->DATA_WIDTH_BYTES);
	493	devc->trigger_value_last = g_malloc0(devc->DATA_WIDTH_BYTES);
a66099c6	494	devc->trigger_edge_mask = g_malloc0(devc->DATA_WIDTH_BYTES);
a4210e18 EK	495	}
	496
	497	SR_PRIV struct ipdbg_org_la_dev_context *ipdbg_org_la_dev_new(void)
	498	{
	499	struct ipdbg_org_la_dev_context *devc;
	500
	501	devc = g_malloc0(sizeof(struct ipdbg_org_la_dev_context));
a4210e18	502	devc->capture_ratio = 50;
a4210e18 EK	503
	504	return devc;
	505	}
	506
7f4c9a04	507	SR_PRIV int ipdbg_org_la_send_reset(struct ipdbg_org_la_tcp *tcp)
a4210e18	508	{
7f4c9a04 SA	509	uint8_t buf = CMD_RESET;
	510	if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
	511	sr_warn("Couldn't send reset");
	512
a4210e18 EK	513	return SR_OK;
	514	}
	515
7f4c9a04	516	SR_PRIV int ipdbg_org_la_request_id(struct ipdbg_org_la_tcp *tcp)
a4210e18	517	{
7f4c9a04 SA	518	uint8_t buf = CMD_GET_LA_ID;
	519	if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
	520	sr_warn("Couldn't send ID request");
a4210e18	521
7f4c9a04 SA	522	char id[4];
	523	if (ipdbg_org_la_tcp_receive_blocking(tcp, (uint8_t*)id, 4) != 4)
	524	sr_warn("Couldn't read device ID");
a4210e18	525
7f4c9a04 SA	526	if (strncmp(id, "IDBG", 4)) {
	527	sr_err("Invalid device ID: expected 'IDBG', got '%c%c%c%c'.",
	528	id[0], id[1], id[2], id[3]);
a4210e18 EK	529	return SR_ERR;
	530	}
	531
	532	return SR_OK;
	533	}
	534
	535	SR_PRIV void ipdbg_org_la_abort_acquisition(const struct sr_dev_inst *sdi)
	536	{
a4210e18 EK	537	struct ipdbg_org_la_tcp *tcp = sdi->conn;
a4210e18 EK	538
2f15f5bf	539	sr_session_source_remove(sdi->session, tcp->socket);
a4210e18	540
b8fa29a1	541	std_session_send_df_end(sdi);
a4210e18 EK	542	}
a4210e18 EK	543
7f4c9a04	544	SR_PRIV int ipdbg_org_la_send_start(struct ipdbg_org_la_tcp *tcp)
a4210e18	545	{
7f4c9a04	546	uint8_t buf = CMD_START;
a4210e18	547
7f4c9a04 SA	548	if (ipdbg_org_la_tcp_send(tcp, &buf, 1) != SR_OK)
	549	sr_warn("Couldn't send start");
	550
	551	return SR_OK;
a4210e18 EK	552	}
a4210e18 EK	553