static void response_send(serial_dfu_t * p_dfu,
serial_dfu_response_t * p_response)
{
uint8_t response_buffer[MAX_RESPONSE_SIZE];
uint8_t encoded_response[MAX_RESPONSE_SIZE*2 + 1];
uint32_t encoded_response_length;
uint16_t index = 0;
NRF_LOG_DEBUG("Sending Response: [0x%01x, 0x%01x]\r\n", p_response->op_code, p_response->resp_val);
response_buffer[index++] = SERIAL_DFU_OP_CODE_RESPONSE;
// Encode the Request Op code
response_buffer[index++] = p_response->op_code;
// Encode the Response Value.
response_buffer[index++] = (uint8_t)p_response->resp_val;
if (p_response->resp_val == NRF_DFU_RES_CODE_SUCCESS)
{
switch (p_response->op_code)
{
case SERIAL_DFU_OP_CODE_CALCULATE_CRC:
index += uint32_encode(p_response->crc_response.offset, &response_buffer[index]);
index += uint32_encode(p_response->crc_response.crc, &response_buffer[index]);
break;
case SERIAL_DFU_OP_CODE_SELECT_OBJECT:
index += uint32_encode(p_response->select_response.max_size, &response_buffer[index]);
index += uint32_encode(p_response->select_response.offset, &response_buffer[index]);
index += uint32_encode(p_response->select_response.crc, &response_buffer[index]);
break;
case SERIAL_DFU_OP_CODE_GET_SERIAL_MTU:
index += uint16_encode(p_response->serial_mtu_response.mtu, &response_buffer[index]);
break;
default:
// no implementation
break;
}
}
// encode into slip
(void)slip_encode(encoded_response, response_buffer, index, &encoded_response_length);
// send
(void)nrf_drv_uart_tx(&p_dfu->uart_instance, encoded_response, encoded_response_length);
}
/* Send a raw slip frame */
int
slip_raw(
struct iface *iface,
struct mbuf **bpp
){
struct mbuf *bp1;
dump(iface,IF_TRACE_OUT,*bpp);
iface->rawsndcnt++;
iface->lastsent = secclock();
if((bp1 = slip_encode(bpp)) == NULL){
return -1;
}
if (iface->trace & IF_TRACE_RAW)
raw_dump(iface,-1,bp1);
return Slip[iface->xdev].send(iface->dev,&bp1);
}
static int send_data(lo_address a, lo_server from, char *data,
const size_t data_len)
{
ssize_t ret = 0;
int sock = -1;
#if defined(WIN32) || defined(_MSC_VER)
if (!initWSock())
return -1;
#endif
if (data_len > LO_MAX_MSG_SIZE) {
a->errnum = 99;
a->errstr = "Attempted to send message in excess of maximum "
"message size";
return -1;
}
// Resolve the destination address, if not done already
if (!a->ai) {
ret = lo_address_resolve(a);
if (ret)
return ret;
}
// Re-use existing socket?
if (from && a->protocol == LO_UDP) {
sock = from->sockets[0].fd;
} else if (a->protocol == LO_UDP && lo_client_sockets.udp != -1) {
sock = lo_client_sockets.udp;
} else {
if (a->socket == -1) {
ret = create_socket(a);
if (ret)
return ret;
// If we are sending TCP, we may later receive on sending
// socket, so add it to the from server's socket list.
if (from && a->protocol == LO_TCP
&& (a->socket >= from->sources_len
|| from->sources[a->socket].host == NULL))
{
lo_server_add_socket(from, a->socket, a, 0, 0);
// If a socket is added to the server, the server is
// now responsible for closing it.
a->ownsocket = 0;
}
}
sock = a->socket;
}
if (a->protocol == LO_TCP && !(a->flags & LO_SLIP)) {
// For TCP only, send the length of the following data
int32_t size = htonl(data_len);
ret = send(sock, (const void*)&size, sizeof(size), MSG_NOSIGNAL);
}
// Send the data
if (ret != -1) {
if (a->protocol == LO_UDP) {
struct addrinfo* ai;
if (a->addr.size == sizeof(struct in_addr)) {
setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
(const char*)&a->addr.a, a->addr.size);
}
#ifdef ENABLE_IPV6
else if (a->addr.size == sizeof(struct in6_addr)) {
setsockopt(sock, IPPROTO_IP, IPV6_MULTICAST_IF,
(const char*)&a->addr.a, a->addr.size);
}
#endif
if (a->ttl >= 0) {
unsigned char ttl = (unsigned char) a->ttl;
setsockopt(sock, IPPROTO_IP, IP_MULTICAST_TTL,
(const char*)&ttl, sizeof(ttl));
}
ai = a->ai;
do {
ret = sendto(sock, data, data_len, MSG_NOSIGNAL,
ai->ai_addr, ai->ai_addrlen);
ai = ai->ai_next;
} while (ret == -1 && ai != NULL);
if (ret == -1 && ai != NULL && a->ai!=ai)
a->ai = ai;
} else {
struct addrinfo* ai = a->ai;
size_t len = data_len;
if (a->flags & LO_SLIP)
data = (char*)slip_encode((unsigned char*)data, &len);
do {
ret = send(sock, data, len, MSG_NOSIGNAL);
if (a->protocol == LO_TCP)
ai = ai->ai_next;
else
ai = 0;
} while (ret == -1 && ai != NULL);
if (ret == -1 && ai != NULL && a->ai!=ai)
a->ai = ai;
if (a->flags & LO_SLIP)
free(data);
}
}
if (ret == -1) {
if (a->protocol == LO_TCP) {
if (from)
lo_server_del_socket(from, -1, a->socket);
closesocket(a->socket);
a->socket = -1;
}
a->errnum = geterror();
a->errstr = NULL;
} else {
a->errnum = 0;
a->errstr = NULL;
}
return ret;
}
void tunnel_event() {
int length, i;
uint8_t * encoded;
size_t encoded_size;
int retries = 0;
printf("Network\n");
// tun always promises to give a full packet which makes life easy
length = read(tunnel, buffer, BUFFER_SIZE);
// write(tunnel, buffer, length);
decode(buffer, length);
for (i = 0; i < length; i++) {
printf("0x%02x ", buffer[i]);
}
printf("\n");
// If someone handed a packet with characters that all needed
// escaping, there would be twice as many characters plus a
// SLIP_END character at the start and end.
encoded_size = length * 2 + 2;
encoded = malloc(encoded_size * sizeof(uint8_t));
// Encode the buffer with slip
length = slip_encode(encoded, encoded_size, buffer, length);
if (length < 0)
warn("Encoded buffer too small");
printf("Encoded: ");
for (i = 0; i < length; i++) {
printf("0x%02x ", encoded[i]);
}
printf("\n");
// Need to IOCTL here to clear DTR (cleared means 1)
ioctl(serial, TIOCCDTR);
// Write the packet to the serial device
for (i = 0; i < length; i++) {
uint8_t received;
// Write the character
write(serial, encoded + i, 1);
// Because the transmit and receive wires are tied, we
// should receive the same character as we sent
read(serial, &received, 1);
printf("0x%x\t0x%x\n", encoded[i], received);
// If they don't match, we've had a collision
if (encoded[i] != received) {
printf("Collision\n");
retries++;
if (retries > MAX_RETRIES) break;
// increase the wait time each time
usleep(random() * retries);
i = 0;
}
// Reset the retries in case we've had collisions
retries = 0;
}
// Set the DTR (ie 0v)
ioctl(serial, TIOCSDTR);
free(encoded);
}
static uint32_t ser_phy_hci_tx_byte()
{
/* Flags informing about actually transmited part of packet*/
static bool header = false;
static bool payload = false;
static bool crc = false;
static bool ack_end = false;
static bool packet_end = false;
if (ack_end == true)
{
ack_end = false;
m_tx_busy = check_pending_tx();
/* Report end of ACK transmission*/
m_ser_phy_hci_slip_event.evt_type = SER_PHY_HCI_SLIP_EVT_ACK_SENT;
m_ser_phy_hci_slip_event_handler(&m_ser_phy_hci_slip_event);
}
else if (packet_end == true)
{
packet_end = false;
m_tx_busy = check_pending_tx();
/* Report end of packet transmission*/
m_ser_phy_hci_slip_event.evt_type = SER_PHY_HCI_SLIP_EVT_PKT_SENT;
m_ser_phy_hci_slip_event_handler(&m_ser_phy_hci_slip_event);
}
else if ((m_tx_index == 0) && !header && !payload && !crc)
{
/* Beginning of packet - sent 0xC0*/
header = true;
mp_data = &m_header;
(void)app_uart_put(APP_SLIP_END);
}
else if ((m_tx_index == mp_data->num_of_bytes) && crc == true)
{
/* End of packet - sent 0xC0*/
(void)app_uart_put(APP_SLIP_END);
m_crc.p_buffer = NULL;
crc = false;
m_tx_index = 0;
packet_end = true;
}
else if ((m_tx_index == mp_data->num_of_bytes) && header == true)
{
/* End of header transmission*/
m_tx_index = 0;
if (m_payload.p_buffer != NULL)
{
header = false;
payload = true;
mp_data = &m_payload;
/* Handle every character in buffer accordingly to SLIP protocol*/
slip_encode();
}
else
{
/* End of ACK - sent 0xC0*/
(void)app_uart_put(APP_SLIP_END);
header = false;
ack_end = true;
}
}
else if ((m_tx_index == mp_data->num_of_bytes) && payload == true)
{
/* End of payload transmission*/
m_tx_index = 0;
if (m_crc.p_buffer != NULL)
{
m_payload.p_buffer = NULL;
payload = false;
crc = true;
mp_data = &m_crc;
/* Handle every character in buffer accordingly to SLIP protocol*/
slip_encode();
}
/* CRC is not used for this packet -> finish packet transmission */
else
{
/* End of packet - send 0xC0*/
(void)app_uart_put(APP_SLIP_END);
m_payload.p_buffer = NULL;
payload = false;
packet_end = true;
}
}
else if (m_tx_escape == false)
{
/* Handle every character in buffer accordingly to SLIP protocol*/
slip_encode();
}
else if (m_tx_escape == true)
{
/* Send SLIP special code*/
m_tx_escape = false;
if (mp_data->p_buffer[m_tx_index] == APP_SLIP_END)
{
(void)app_uart_put(APP_SLIP_ESC_END);
m_tx_index++;
}
else
{
(void)app_uart_put(APP_SLIP_ESC_ESC);
m_tx_index++;
}
}
return NRF_SUCCESS;
}
unsigned short MakeDataPacket(sampleData_t* sample , unsigned char mode, void** start)
{
// Get sample
static char output[100]; // Ensure you dont overrun this buffer
char *p = output;
unsigned short num = 0;
// Ascii mode
if ((mode == 0)||(mode == 128))
{
// ascii mode
// Print string
p += my_nitoa(p, sample->sampleCount); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.accel.z); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.gyro.z); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.x); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.y); *p++ = ',';
p += my_nitoa(p, sample->sensor.mag.z);
// If top bit set then append extra data
if ((mode & 0x80)||(status.newXdata==1))
{
*p++ = ',';
p += my_nitoa(p, status.battmv); *p++ = ',';
p += my_nitoa(p, status.temperature); *p++ = ',';
{
char * ptr = (char*)my_ultoa(status.pressure);
unsigned char len = strlen(ptr);
memcpy(p,ptr,len);
p+=len;
*p++ = ',';
}
p += my_nitoa(p, status.inactivity);
}
*p++ = '\r';*p++ = '\n'; *p = '\0';
// Count bytes
num = p - output;
}
// Binary slip mode
if (mode == 1)
{
// binary mode 1
*p++ = SLIP_END;
*p++ = NINE_AXIS_PACKET;
if (status.newXdata==1) *p++ = PACKET_V2;
else *p++ = PACKET_V1;
// KL: to keep compatibility with original format
// (TODO, migrate to better format with sample at end aswell for better framing)
p += slip_encode(p, &sample->sampleCount, sizeof(unsigned short));
p += slip_encode(p, &sample->sampleTicks, sizeof(unsigned long));
p += slip_encode(p, &sample->sensor, sizeof(sensor_t));
if (status.newXdata == 1)
{
p += slip_encode(p, &status.battmv, sizeof(unsigned short));
p += slip_encode(p, &status.temperature, sizeof(short));
p += slip_encode(p, &status.pressure, sizeof(unsigned long));
}
*p++ = SLIP_END;
// Count bytes
num = p - output;
}
// 20 byte special mode for ble attribute notifications
if (mode == 2)
{
// 20 Byte payload for BLE applications
// Copy sample to static
memcpy(&output[0],sample,(sizeof(sampleData_t)-4)); // 20
// Set number of bytes
num = 20;
}
// Set pointer to data
*start = output;
return num;
}