コード例 #1
0
ファイル: csp_port.c プロジェクト: jtrutna/libcsp
int csp_bind(csp_socket_t * socket, uint8_t port) {
	
	if (socket == NULL)
		return CSP_ERR_INVAL;

	if (port > CSP_ANY) {
		csp_log_error("Only ports from 0-%u (and CSP_ANY for default) are available for incoming ports\r\n", CSP_ANY);
		return CSP_ERR_INVAL;
	}

	/* Check if port number is valid */
	if (ports[port].state != PORT_CLOSED) {
		csp_log_error("Port %d is already in use\r\n", port);
		return CSP_ERR_USED;
	}

	csp_log_info("Binding socket %p to port %u\r\n", socket, port);

	/* Save listener */
	ports[port].socket = socket;
	ports[port].state = PORT_OPEN;

	return CSP_ERR_NONE;

}
コード例 #2
0
ファイル: csp_buffer.c プロジェクト: bert/libcsp
void csp_buffer_free(void *packet) {
	if (!packet) {
		csp_log_error("Attempt to free null pointer\r\n");
		return;
	}

	csp_skbf_t * buf = packet - sizeof(csp_skbf_t);

	if (((uintptr_t) buf % CSP_BUFFER_ALIGN) > 0) {
		csp_log_error("FREE: Unaligned CSP buffer pointer %p\r\n", packet);
		return;
	}

	if (buf->skbf_addr != buf) {
		csp_log_error("FREE: Invalid CSP buffer pointer %p\r\n", packet);
		return;
	}

	if (buf->refcount == 0) {
		csp_log_error("FREE: Buffer already free %p\r\n", buf);
		return;
	} else if (buf->refcount > 1) {
		buf->refcount--;
		csp_log_error("FREE: Buffer %p in use by %u users\r\n", buf, buf->refcount);
		return;
	} else {
		buf->refcount = 0;
		csp_log_buffer("FREE: %p\r\n", buf);
		csp_queue_enqueue(csp_buffers, &buf, 0);
	}

}
コード例 #3
0
ファイル: csp_route.c プロジェクト: GomSpace/libcsp
/**
 * Check supported packet options
 * @param interface pointer to incoming interface
 * @param packet pointer to packet
 * @return CSP_ERR_NONE is all options are supported, CSP_ERR_NOTSUP if not
 */
static int csp_route_check_options(csp_iface_t *interface, csp_packet_t *packet)
{
#ifndef CSP_USE_XTEA
	/* Drop XTEA packets */
	if (packet->id.flags & CSP_FXTEA) {
		csp_log_error("Received XTEA encrypted packet, but CSP was compiled without XTEA support. Discarding packet");
		interface->autherr++;
		return CSP_ERR_NOTSUP;
	}
#endif

#ifndef CSP_USE_HMAC
	/* Drop HMAC packets */
	if (packet->id.flags & CSP_FHMAC) {
		csp_log_error("Received packet with HMAC, but CSP was compiled without HMAC support. Discarding packet");
		interface->autherr++;
		return CSP_ERR_NOTSUP;
	}
#endif

#ifndef CSP_USE_RDP
	/* Drop RDP packets */
	if (packet->id.flags & CSP_FRDP) {
		csp_log_error("Received RDP packet, but CSP was compiled without RDP support. Discarding packet");
		interface->rx_error++;
		return CSP_ERR_NOTSUP;
	}
#endif
	return CSP_ERR_NONE;
}
コード例 #4
0
ファイル: can_socketcan.c プロジェクト: nicob87/libcsp
int can_mbox_init(can_iface_ctx_t *iface_ctx) {

    int i;
    mbox_t *m, *mbox;

    mbox = iface_ctx->mbox;

    for (i = 0; i < MBOX_NUM; i++) {
        m = &mbox[i];
        m->state = MBOX_FREE;
        m->csp_can_socket = &iface_ctx->csp_can_socket;
        m->mbox_pool_sem = &iface_ctx->mbox_pool_sem;

        /* Init signal semaphore */
        if (csp_bin_sem_create(&(m->signal_sem)) != CSP_SEMAPHORE_OK) {
            csp_log_error("sem create");
            return -1;
        } else {
            /* Take signal semaphore so the thread waits for tx data */
            csp_bin_sem_wait(&(m->signal_sem), CSP_MAX_DELAY);
        }

        /* Create mailbox */
        if(csp_thread_create(mbox_tx_thread, (signed char *)"mbox_tx", 1024, (void *)m, 3,  &m->thread) != CSP_ERR_NONE) { //TODO: Adjust priority
            csp_log_error("thread creation");
            return -1;
        }
    }

    /* Init mailbox pool semaphore */
    csp_bin_sem_create(&iface_ctx->mbox_pool_sem);
    return 0;

}
コード例 #5
0
ファイル: can_socketcan.c プロジェクト: AdamPrzybyla/libcsp
int can_mbox_init(void) {

	int i;
	mbox_t * m;

	for (i = 0; i < MBOX_NUM; i++) {
		m = &mbox[i];
		m->state = MBOX_FREE;

		/* Init signal semaphore */
		if (sem_init(&(m->signal_sem), 0, 1) != 0) {
			csp_log_error("sem_init: %s\r\n", strerror(errno));
			return -1;
		} else {
			/* Take signal semaphore so the thread waits for tx data */
			sem_wait(&(m->signal_sem));
		}

		/* Create mailbox */
		if (pthread_create(&m->thread, NULL, mbox_tx_thread, (void *)m) != 0) {
			csp_log_error("pthread_create: %s\r\n", strerror(errno));
			return -1;
		}
	}

	/* Init mailbox pool semaphore */
	sem_init(&mbox_sem, 0, 1);

	return 0;

}
コード例 #6
0
ファイル: csp_buffer.c プロジェクト: bert/libcsp
void *csp_buffer_get(size_t buf_size) {

	csp_skbf_t * buffer = NULL;

	if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
		csp_log_error("Attempt to allocate too large block %u\r\n", buf_size);
		return NULL;
	}

	csp_queue_dequeue(csp_buffers, &buffer, 0);
	if (buffer == NULL) {
		csp_log_error("Out of buffers\r\n");
		return NULL;
	}

	csp_log_buffer("GET: %p %p\r\n", buffer, buffer->skbf_addr);

	if (buffer != buffer->skbf_addr) {
		csp_log_error("Corrupt CSP buffer\r\n");
		return NULL;
	}

	buffer->refcount++;
	return buffer->skbf_data;
}
コード例 #7
0
ファイル: csp_rtable_cidr.c プロジェクト: CatFreed/libcsp
static int csp_rtable_parse(char * buffer, int dry_run) {

	int valid_entries = 0;

	/* Copy string before running strtok */
	char * str = alloca(strlen(buffer) + 1);
	memcpy(str, buffer, strlen(buffer) + 1);

	/* Get first token */
	str = strtok(str, ",");

	while ((str) && (strlen(str) > 1)) {
		int address = 0, netmask = 0, mac = 255;
		char name[100] = {};
		if (sscanf(str, "%u/%u %s %u", &address, &netmask, name, &mac) != 4) {
			if (sscanf(str, "%u/%u %s", &address, &netmask, name) != 3) {
				csp_log_error("Parse error %s", str);
				return -1;
			}
		}
		//printf("Parsed %u/%u %u %s\r\n", address, netmask, mac, name);
		csp_iface_t * ifc = csp_iflist_get_by_name(name);
		if (ifc) {
			if (dry_run == 0)
				csp_rtable_set(address, netmask, ifc, mac);
		} else {
			csp_log_error("Unknown interface %s", name);
			return -1;
		}
		valid_entries++;
		str = strtok(NULL, ",");
	}

	return valid_entries;
}
コード例 #8
0
ファイル: can_at90can128.c プロジェクト: TiagoDemay/libcsp
int can_configure_bitrate(unsigned long int afcpu, uint32_t bps) {

	/* Set baud rate (500 kbps) */
	if (bps != 500000) {
		csp_log_error("CAN bitrate must be 500000 (was %d)\r\n");
		return -1;
	}

	/* TODO: Set arbitrary bitrate */
	if (afcpu == 8000000) {
		CANTCON = 0x02;	  // Set CAN Timer Prescaler
		CANBT1 = 0x02;	   //!< Tscl  = 2x Tclkio = 250 ns
		CANBT2 = 0x04;	   //!< Tsync = 1x Tscl, Tprs = 3x Tscl, Tsjw = 1x Tscl
		CANBT3 = 0x13;	   //!< Tpsh1 = 2x Tscl, Tpsh2 = 2x Tscl, 3 sample points
	} else if (afcpu == 12000000) {
		CANTCON = 0x02;	  // Set CAN Timer Prescaler
		CANBT1 = 0x02;	   //!< Tscl  = 2x Tclkio = 166.666 ns
		CANBT2 = 0x08;	   //!< Tsync = 1x Tscl, Tprs = 5x Tscl, Tsjw = 1x Tscl
		CANBT3 = 0x25;	   //!< Tpsh1 = 3x Tscl, Tpsh2 = 3x Tscl, 3 sample points
	} else if (afcpu == 16000000) {
		CANTCON = 128;	   // Set CAN Timer Prescaler
		CANBT1 = 0x06;	   //!< Tscl  = 4x Tclkio = 250 ns
		CANBT2 = 0x04;	   //!< Tsync = 1x Tscl, Tprs = 3x Tscl, Tsjw = 1x Tscl
		CANBT3 = 0x13;	   //!< Tpsh1 = 2x Tscl, Tpsh2 = 2x Tscl, 3 sample points
	} else {
		csp_log_error("Error, missing CAN driver defines for that FCPU=%d\r\n", afcpu);
		return -1;
	}

	return 0;

}
コード例 #9
0
ファイル: csp_if_can.c プロジェクト: ArduSat/libcsp_migrated
/** pbuf_init
 * Initialize packet buffer.
 * @return 0 on success, -1 on error.
 */
static int pbuf_init(void) {

	/* Initialize packet buffers */
	int i;
	pbuf_element_t *buf;

	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];
		buf->rx_count = 0;
		buf->tx_count = 0;
		buf->cfpid = 0;
		buf->packet = NULL;
		buf->state = BUF_FREE;
		buf->last_used = 0;
		buf->remain = 0;
		/* Create tx semaphore if blocking mode is enabled */
		if (csp_bin_sem_create(&buf->tx_sem) != CSP_SEMAPHORE_OK) {
			csp_log_error("Failed to allocate TX semaphore\r\n");
			return CSP_ERR_NOMEM;
		}
	}

	/* Initialize global lock */
	if (CSP_INIT_CRITICAL(pbuf_sem) != CSP_ERR_NONE) {
		csp_log_error("No more memory for packet buffer semaphore\r\n");
		return CSP_ERR_NOMEM;
	}

	return CSP_ERR_NONE;

}
コード例 #10
0
ファイル: csp_io.c プロジェクト: janbre/NUTS
int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) {

    packet->id.flags = 0;

    if (opts & CSP_O_RDP) {
        csp_log_error("Attempt to create RDP packet on connection-less socket\r\n");
        return CSP_ERR_INVAL;
    }

    /** SEQNR */
    if(opts & CSP_O_SEQNR) {
#ifdef CSP_USE_SEQNR
        packet->id.flags |= CSP_SEQNR;
#else
        csp_log_error("Attempt to create packet with SEQNR capabilities, but CSP was compiled without such support\r\n");
        return CSP_ERR_NOTSUP;
#endif
    }

    if (opts & CSP_O_HMAC) {
#ifdef CSP_USE_HMAC
        packet->id.flags |= CSP_FHMAC;
#else
        csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support\r\n");
        return CSP_ERR_NOTSUP;
#endif
    }

    if (opts & CSP_O_XTEA) {
#ifdef CSP_USE_XTEA
        packet->id.flags |= CSP_FXTEA;
#else
        csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support\r\n");
        return CSP_ERR_NOTSUP;
#endif
    }

    if (opts & CSP_O_CRC32) {
#ifdef CSP_USE_CRC32
        packet->id.flags |= CSP_FCRC32;
#else
        csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support\r\n");
        return CSP_ERR_NOTSUP;
#endif
    }

    packet->id.dst = dest;
    packet->id.dport = dport;
    packet->id.src = my_address;
    packet->id.sport = src_port;
    packet->id.pri = prio;

    if (csp_send_direct(packet->id, packet, timeout) != CSP_ERR_NONE)
        return CSP_ERR_NOTSUP;

    return CSP_ERR_NONE;
}
コード例 #11
0
ファイル: csp_io.c プロジェクト: keimi/SUCHAI
csp_socket_t * csp_socket(uint32_t opts) {
	
	/* Validate socket options */
#ifndef CSP_USE_RDP
	if (opts & CSP_SO_RDPREQ) {
		csp_log_error("Attempt to create socket that requires RDP, but CSP was compiled without RDP support\r\n");
		return NULL;
	}
#endif

#ifndef CSP_USE_XTEA
	if (opts & CSP_SO_XTEAREQ) {
		csp_log_error("Attempt to create socket that requires XTEA, but CSP was compiled without XTEA support\r\n");
		return NULL;
	}
#endif

#ifndef CSP_USE_HMAC
	if (opts & CSP_SO_HMACREQ) {
		csp_log_error("Attempt to create socket that requires HMAC, but CSP was compiled without HMAC support\r\n");
		return NULL;
	} 
#endif

#ifndef CSP_USE_CRC32
	if (opts & CSP_SO_CRC32REQ) {
		csp_log_error("Attempt to create socket that requires CRC32, but CSP was compiled without CRC32 support\r\n");
		return NULL;
	} 
#endif
	
	/* Drop packet if reserved flags are set */
	if (opts & ~(CSP_SO_RDPREQ | CSP_SO_XTEAREQ | CSP_SO_HMACREQ | CSP_SO_CRC32REQ | CSP_SO_CONN_LESS)) {
		csp_log_error("Invalid socket option\r\n");
		return NULL;
	}

	/* Use CSP buffers instead? */
	csp_socket_t * sock = csp_conn_allocate(CONN_SERVER);
	if (sock == NULL)
		return NULL;

	/* If connectionless, init the queue to a pre-defined size
	 * if not, the user must init the queue using csp_listen */
	if (opts & CSP_SO_CONN_LESS) {
		sock->socket = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(csp_packet_t *));
		if (sock->socket == NULL)
			return NULL;
	} else {
		sock->socket = NULL;
	}
	sock->opts = opts;

	return sock;

}
コード例 #12
0
ファイル: csp_io.c プロジェクト: CatFreed/libcsp
int csp_sendto(uint8_t prio, uint8_t dest, uint8_t dport, uint8_t src_port, uint32_t opts, csp_packet_t * packet, uint32_t timeout) {

	packet->id.flags = 0;

	if (opts & CSP_O_RDP) {
		csp_log_error("Attempt to create RDP packet on connection-less socket");
		return CSP_ERR_INVAL;
	}

	if (opts & CSP_O_HMAC) {
#ifdef CSP_USE_HMAC
		packet->id.flags |= CSP_FHMAC;
#else
		csp_log_error("Attempt to create HMAC authenticated packet, but CSP was compiled without HMAC support");
		return CSP_ERR_NOTSUP;
#endif
	}

	if (opts & CSP_O_XTEA) {
#ifdef CSP_USE_XTEA
		packet->id.flags |= CSP_FXTEA;
#else
		csp_log_error("Attempt to create XTEA encrypted packet, but CSP was compiled without XTEA support");
		return CSP_ERR_NOTSUP;
#endif
	}

	if (opts & CSP_O_CRC32) {
#ifdef CSP_USE_CRC32
		packet->id.flags |= CSP_FCRC32;
#else
		csp_log_error("Attempt to create CRC32 validated packet, but CSP was compiled without CRC32 support");
		return CSP_ERR_NOTSUP;
#endif
	}

	packet->id.dst = dest;
	packet->id.dport = dport;
	packet->id.src = csp_get_address();
	packet->id.sport = src_port;
	packet->id.pri = prio;

	csp_iface_t * ifout = csp_rtable_find_iface(dest);
	if (csp_send_direct(packet->id, packet, ifout, timeout) != CSP_ERR_NONE)
		return CSP_ERR_NOTSUP;
	
	return CSP_ERR_NONE;

}
コード例 #13
0
ファイル: csp_io.c プロジェクト: janbre/NUTS
int csp_send(csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) {

    int ret;

    if ((conn == NULL) || (packet == NULL) || (conn->state != CONN_OPEN)) {
        csp_log_error("Invalid call to csp_send\r\n");
        return 0;
    }

#ifdef CSP_USE_RDP
    if (conn->idout.flags & CSP_FRDP) {
        if (csp_rdp_send(conn, packet, timeout) != CSP_ERR_NONE) {
            csp_route_t * ifout = csp_route_if(conn->idout.dst);
            if (ifout != NULL && ifout->interface != NULL)
                ifout->interface->tx_error++;
            csp_log_warn("RDP send failed\r\n!");
            return 0;
        }
    }
#endif

    ret = csp_send_direct(conn->idout, packet, timeout);

    return (ret == CSP_ERR_NONE) ? 1 : 0;

}
コード例 #14
0
ファイル: csp_route.c プロジェクト: janbre/NUTS
int csp_route_set(uint8_t node, csp_iface_t *ifc, uint8_t nexthop_mac_addr) {

    /* Don't add nothing */
    if (ifc == NULL)
        return CSP_ERR_INVAL;

    /**
     * Check if the interface has been added.
     *
     * NOTE: For future implementations, interfaces should call
     * csp_route_add_if in its csp_if_<name>_init function, instead
     * of registering at first route_set, in order to make the interface
     * available to network based (CMP) route configuration.
     */
    csp_route_add_if(ifc);

    /* Set route */
    if (node <= CSP_DEFAULT_ROUTE) {
        routes[node].interface = ifc;
        routes[node].nexthop_mac_addr = nexthop_mac_addr;
    } else {
        csp_log_error("Failed to set route: invalid node id %u\r\n", node);
        return CSP_ERR_INVAL;
    }

    return CSP_ERR_NONE;

}
コード例 #15
0
ファイル: csp_buffer.c プロジェクト: janbre/NUTS
void *csp_buffer_get(size_t buf_size) {
	void *buffer;

	if (buf_size + CSP_BUFFER_PACKET_OVERHEAD > size) {
		csp_log_error("Attempt to allocate too large block %u\r\n", buf_size);
		return NULL;
	}

	csp_queue_dequeue(csp_buffers, &buffer, 0);

	if (buffer != NULL) {
		csp_log_buffer("BUFFER: Using element at %p\r\n", buffer);
	} else {
		csp_log_error("Out of buffers\r\n");
	}

	return buffer;
}
コード例 #16
0
ファイル: csp_buffer.c プロジェクト: ArduSat/libcsp_migrated
int csp_buffer_get_refcount(void *packet) {
	if (!packet) {
		csp_log_error("Attempt to free null pointer\r\n");
		return -1;
	}

	csp_skbf_t * buf = packet - sizeof(csp_skbf_t);
    return buf->refcount;
}
コード例 #17
0
ファイル: csp_conn.c プロジェクト: tbyerinn/libcsp
int csp_close(csp_conn_t * conn) {

    if (conn == NULL) {
        csp_log_error("NULL Pointer given to csp_close\r\n");
        return CSP_ERR_INVAL;
    }

    if (conn->state == CONN_CLOSED) {
        csp_log_protocol("Conn already closed\r\n");
        return CSP_ERR_NONE;
    }

#ifdef CSP_USE_RDP
    /* Ensure RDP knows this connection is closing */
    if (conn->idin.flags & CSP_FRDP || conn->idout.flags & CSP_FRDP)
        if (csp_rdp_close(conn) == CSP_ERR_AGAIN)
            return CSP_ERR_NONE;
#endif

    /* Lock connection array while closing connection */
    if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
        csp_log_error("Failed to lock conn array\r\n");
        return CSP_ERR_TIMEDOUT;
    }

    /* Set to closed */
    conn->state = CONN_CLOSED;

    /* Ensure connection queue is empty */
    csp_conn_flush_rx_queue(conn);

    /* Reset RDP state */
#ifdef CSP_USE_RDP
    if (conn->idin.flags & CSP_FRDP)
        csp_rdp_flush_all(conn);
#endif

    /* Unlock connection array */
    csp_bin_sem_post(&conn_lock);

    return CSP_ERR_NONE;
}
コード例 #18
0
ファイル: csp_conn.c プロジェクト: tbyerinn/libcsp
int csp_conn_init(void) {

    /* Initialize source port */
    srand(csp_get_ms());
    sport = (rand() % (CSP_ID_PORT_MAX - CSP_MAX_BIND_PORT)) + (CSP_MAX_BIND_PORT + 1);

    if (csp_bin_sem_create(&sport_lock) != CSP_SEMAPHORE_OK) {
        csp_log_error("No more memory for sport semaphore\r\n");
        return CSP_ERR_NOMEM;
    }

    int i, prio;
    for (i = 0; i < CSP_CONN_MAX; i++) {
        for (prio = 0; prio < CSP_RX_QUEUES; prio++)
            arr_conn[i].rx_queue[prio] = csp_queue_create(CSP_RX_QUEUE_LENGTH, sizeof(csp_packet_t *));

#ifdef CSP_USE_QOS
        arr_conn[i].rx_event = csp_queue_create(CSP_CONN_QUEUE_LENGTH, sizeof(int));
#endif
        arr_conn[i].state = CONN_CLOSED;

        if (csp_mutex_create(&arr_conn[i].lock) != CSP_MUTEX_OK) {
            csp_log_error("Failed to create connection lock\r\n");
            return CSP_ERR_NOMEM;
        }

#ifdef CSP_USE_RDP
        if (csp_rdp_allocate(&arr_conn[i]) != CSP_ERR_NONE) {
            csp_log_error("Failed to create queues for RDP in csp_conn_init\r\n");
            return CSP_ERR_NOMEM;
        }
#endif
    }

    if (csp_bin_sem_create(&conn_lock) != CSP_SEMAPHORE_OK) {
        csp_log_error("No more memory for conn semaphore\r\n");
        return CSP_ERR_NOMEM;
    }

    return CSP_ERR_NONE;

}
コード例 #19
0
ファイル: csp_system.c プロジェクト: CatFreed/libcsp
int csp_sys_shutdown(void) {

	extern void __attribute__((weak)) cpu_shutdown(void);
	if (cpu_shutdown) {
		cpu_shutdown();
		while (1);
	}

	csp_log_error("Failed to shutdown");

	return CSP_ERR_INVAL;
}
コード例 #20
0
ファイル: csp_route.c プロジェクト: janbre/NUTS
int csp_route_start_task(unsigned int task_stack_size, unsigned int priority) {

    int ret = csp_thread_create(csp_task_router, (signed char *) "RTE", task_stack_size, NULL, priority, &handle_router);

    if (ret != 0) {
        csp_log_error("Failed to start router task\n");
        return CSP_ERR_NOMEM;
    }

    return CSP_ERR_NONE;

}
コード例 #21
0
ファイル: can_socketcan.c プロジェクト: AdamPrzybyla/libcsp
int can_init(uint32_t id, uint32_t mask, can_tx_callback_t atxcb, can_rx_callback_t arxcb, struct csp_can_config *conf) {

	struct ifreq ifr;
	struct sockaddr_can addr;
	pthread_t rx_thread;

	csp_assert(conf && conf->ifc);

	txcb = atxcb;
	rxcb = arxcb;

	/* Create socket */
	if ((can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
		csp_log_error("socket: %s\r\n", strerror(errno));
		return -1;
	}

	/* Locate interface */
	strncpy(ifr.ifr_name, conf->ifc, IFNAMSIZ - 1);
	if (ioctl(can_socket, SIOCGIFINDEX, &ifr) < 0) {
		csp_log_error("ioctl: %s\r\n", strerror(errno));
		return -1;
	}

	/* Bind the socket to CAN interface */
	addr.can_family = AF_CAN;
	addr.can_ifindex = ifr.ifr_ifindex;
	if (bind(can_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
		csp_log_error("bind: %s\r\n", strerror(errno));
		return -1;
	}

	/* Set promiscuous mode */
	if (mask) {
		struct can_filter filter;
		filter.can_id   = id;
		filter.can_mask = mask;
		if (setsockopt(can_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
			csp_log_error("setsockopt: %s\r\n", strerror(errno));
			return -1;
		}
	}

	/* Create receive thread */
	if (pthread_create(&rx_thread, NULL, mbox_rx_thread, NULL) != 0) {
		csp_log_error("setsockopt: %s\r\n", strerror(errno));
		return -1;
	}

	/* Create mailbox pool */
	if (can_mbox_init() != 0) {
		csp_log_error("Failed to create tx thread pool\n");
		return -1;
	}

	return 0;

}
コード例 #22
0
ファイル: csp_if_can.c プロジェクト: ArduSat/libcsp_migrated
/* Identification number */
static int id_init(void) {

	/* Init ID field to random number */
	srand((int)csp_get_ms());
	cfp_id = rand() & ((1 << CFP_ID_SIZE) - 1);

	if (csp_bin_sem_create(&id_sem) == CSP_SEMAPHORE_OK) {
		return CSP_ERR_NONE;
	} else {
		csp_log_error("Could not initialize CFP id semaphore\r\n");
		return CSP_ERR_NOMEM;
	}

}
コード例 #23
0
ファイル: csp_conn.c プロジェクト: tbyerinn/libcsp
csp_conn_t * csp_conn_allocate(csp_conn_type_t type) {

    int i, j;
    static uint8_t csp_conn_last_given = 0;
    csp_conn_t * conn;

    if (csp_bin_sem_wait(&conn_lock, 100) != CSP_SEMAPHORE_OK) {
        csp_log_error("Failed to lock conn array\r\n");
        return NULL;
    }

    /* Search for free connection */
    i = csp_conn_last_given;
    i = (i + 1) % CSP_CONN_MAX;

    for (j = 0; j < CSP_CONN_MAX; j++) {
        conn = &arr_conn[i];
        if (conn->state == CONN_CLOSED)
            break;
        i = (i + 1) % CSP_CONN_MAX;
    }

    if (conn->state == CONN_OPEN) {
        csp_log_error("No more free connections\r\n");
        csp_bin_sem_post(&conn_lock);
        return NULL;
    }

    conn->state = CONN_OPEN;
    conn->socket = NULL;
    conn->type = type;
    csp_conn_last_given = i;
    csp_bin_sem_post(&conn_lock);

    return conn;

}
コード例 #24
0
ファイル: csp_system.c プロジェクト: CatFreed/libcsp
int csp_sys_reboot(void) {

	extern void __attribute__((weak)) cpu_set_reset_cause(unsigned int);
	if (cpu_set_reset_cause)
		cpu_set_reset_cause(1);
	
	extern void __attribute__((weak)) cpu_reset(void);
	if (cpu_reset) {
		cpu_reset();
		while (1);
	}
	
	csp_log_error("Failed to reboot");

	return CSP_ERR_INVAL;
}
コード例 #25
0
ファイル: csp_route.c プロジェクト: nsat/libcsp
void csp_promisc_add(csp_packet_t * packet, csp_queue_handle_t queue) {

	if (csp_promisc_enabled == 0)
		return;

	if (queue != NULL) {
		/* Make a copy of the message and queue it to the promiscuous task */
		csp_packet_t *packet_copy = csp_buffer_clone(packet);
		if (packet_copy != NULL) {
			if (csp_queue_enqueue(queue, &packet_copy, 0) != CSP_QUEUE_OK) {
				csp_log_error("Promiscuous mode input queue full\r\n");
				csp_buffer_free(packet_copy);
			}
		}
	}

}
コード例 #26
0
ファイル: csp_bridge.c プロジェクト: CatFreed/libcsp
int csp_bridge_start(unsigned int task_stack_size, unsigned int task_priority, csp_iface_t * _if_a, csp_iface_t * _if_b) {

	/* Set static references to A/B side of bridge */
	if_a = _if_a;
	if_b = _if_b;

	static csp_thread_handle_t handle;
	int ret = csp_thread_create(csp_bridge, "BRIDGE", task_stack_size, NULL, task_priority, &handle);

	if (ret != 0) {
		csp_log_error("Failed to start task");
		return CSP_ERR_NOMEM;
	}

	return CSP_ERR_NONE;

}
コード例 #27
0
ファイル: csp_if_can.c プロジェクト: ArduSat/libcsp_migrated
int csp_can_init(uint8_t mode, struct csp_can_config *conf) {

	int ret;
	uint32_t mask;

	/* Initialize packet buffer */
	if (pbuf_init() != 0) {
		csp_log_error("Failed to initialize CAN packet buffers\r\n");
		return CSP_ERR_NOMEM;
	}

	/* Initialize CFP identifier */
	if (id_init() != 0) {
		csp_log_error("Failed to initialize CAN identification number\r\n");
		return CSP_ERR_NOMEM;
	}

	if (mode == CSP_CAN_MASKED) {
		mask = CFP_MAKE_DST((1 << CFP_HOST_SIZE) - 1);
	} else if (mode == CSP_CAN_PROMISC) {
		mask = 0;
		csp_if_can.promisc = 1;
	} else {
		csp_log_error("Unknown CAN mode\r\n");
		return CSP_ERR_INVAL;
	}

	can_rx_queue = csp_queue_create(CSP_CAN_RX_QUEUE_SIZE, sizeof(can_frame_t));
	if (can_rx_queue == NULL) {
		csp_log_error("Failed to create CAN RX queue\r\n");
		return CSP_ERR_NOMEM;
	}

	ret = csp_thread_create(csp_can_rx_task, (signed char *) "CANRX",2048, NULL, 3, &can_rx_task);
	if (ret != 0) {
		csp_log_error("Failed to init CAN RX task\r\n");
		return CSP_ERR_NOMEM;
	}

	/* Initialize CAN driver */
	if (can_init(CFP_MAKE_DST(my_address), mask, csp_tx_callback, csp_rx_callback, conf) != 0) {
		csp_log_error("Failed to initialize CAN driver\r\n");
		return CSP_ERR_DRIVER;
	}

	/* Regsiter interface */
	csp_route_add_if(&csp_if_can);

	return CSP_ERR_NONE;

}
コード例 #28
0
ファイル: can_socketcan.c プロジェクト: AdamPrzybyla/libcsp
/* Mailbox thread */
static void * mbox_tx_thread(void * parameters) {

	/* Set thread parameters */
	mbox_t * m = (mbox_t *)parameters;

	uint32_t id;

	while (1) {

		/* Wait for a new packet to process */
		sem_wait(&(m->signal_sem));

		/* Send frame */
		int tries = 0, error = CAN_NO_ERROR;
		while (write(can_socket, &m->frame, sizeof(m->frame)) != sizeof(m->frame)) {
			if (++tries < 1000 && errno == ENOBUFS) {
				/* Wait 10 ms and try again */
				usleep(10000);
			} else {
				csp_log_error("write: %s\r\n", strerror(errno));
				error = CAN_ERROR;
				break;
			}
		}

		id = m->frame.can_id;

		/* Free mailbox */
		sem_wait(&mbox_sem);
		m->state = MBOX_FREE;
		sem_post(&mbox_sem);
		
		/* Call tx callback */
		if (txcb) txcb(id, error, NULL);

	}

	/* We should never reach this point */
	pthread_exit(NULL);

}
コード例 #29
0
ファイル: csp_udp.c プロジェクト: AdamPrzybyla/libcsp
void csp_udp_new_packet(csp_conn_t * conn, csp_packet_t * packet) {

    /* Enqueue */
    if (csp_conn_enqueue_packet(conn, packet) < 0) {
        csp_log_error("Connection buffer queue full!\r\n");
        csp_buffer_free(packet);
        return;
    }

    /* Try to queue up the new connection pointer */
    if (conn->socket != NULL) {
        if (csp_queue_enqueue(conn->socket, &conn, 0) != CSP_QUEUE_OK) {
            csp_log_warn("Warning socket connection queue full\r\n");
            csp_close(conn);
            return;
        }

        /* Ensure that this connection will not be posted to this socket again */
        conn->socket = NULL;
    }

}
コード例 #30
0
ファイル: csp_io.c プロジェクト: keimi/SUCHAI
int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {

	int size = (inlen > outlen) ? inlen : outlen;
	csp_packet_t * packet = csp_buffer_get(size);
	if (packet == NULL)
		return 0;

	/* Copy the request */
	if (outlen > 0 && outbuf != NULL)
		memcpy(packet->data, outbuf, outlen);
	packet->length = outlen;

	if (!csp_send(conn, packet, timeout)) {
		csp_buffer_free(packet);
		return 0;
	}

	/* If no reply is expected, return now */
	if (inlen == 0)
        {
		return 1;
        }

	packet = csp_read(conn, timeout);
	if (packet == NULL)
		return 0;

	if ((inlen != -1) && ((int)packet->length != inlen)) {
		csp_log_error("Reply length %u expected %u\r\n", packet->length, inlen);
		csp_buffer_free(packet);
		return 0;
	}

	memcpy(inbuf, packet->data, packet->length);
	int length = packet->length;
	csp_buffer_free(packet);
	return length;

}