OrgApache/incubator-nuttx
OrgApache
/
incubator-nuttx
mirror of https://github.com/apache/incubator-nuttx.git
1
0
Fork 0
Code Issues Releases Wiki Activity
incubator-nuttx/drivers/modem/alt1250/altcom_pkt.c

434 lines
12 KiB
C
Raw Blame History

/****************************************************************************
* drivers/modem/alt1250/altcom_pkt.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/modem/alt1250.h>
#include <assert.h>
#include "altcom_pkt.h"
/****************************************************************************
* Private Types
****************************************************************************/
struct calculated_checksum
{
uint16_t header_checksum;
uint16_t body_checksum;
};
/****************************************************************************
* Private Data
****************************************************************************/
static uint8_t g_poweron_cmd[sizeof(struct altcom_cmdhdr_s) +
sizeof(struct altcom_cmdfooter_s)];
static uint8_t g_seqid;
static uint16_t g_transid;
/****************************************************************************
* Private Functions
****************************************************************************/
static uint16_t calc_checksum_v1(FAR uint8_t *ptr, uint16_t len)
{
uint32_t ret = 0x00;
uint16_t calctmp = 0x00;
uint16_t i;
int is_odd = len & 0x01;
for (i = 0; i < (len & 0xfffe); i += sizeof(uint16_t))
{
calctmp = *((uint16_t *)(ptr + i));
ret += ntohs(calctmp);
}
if (is_odd)
{
ret += *(ptr + i) << 8;
}
ret = ~((ret & 0xffff) + (ret >> 16));
return (uint16_t)ret;
}
static uint16_t calc_checksum_v4(FAR uint8_t *ptr, uint16_t len)
{
uint32_t ret = 0x00;
uint32_t calctmp = 0x00;
uint16_t i;
/* Data accumulating */
for (i = 0; i < len; i++)
{
calctmp += ptr[i];
}
ret = ~((calctmp & 0xffff) + (calctmp >> 16));
return (uint16_t)ret;
}
static inline void set_header_top(FAR struct altcom_cmdhdr_s *hdr,
uint8_t ver, uint16_t cid)
{
hdr->magic = htonl(ALTCOM_HDR_MAGICNUMBER);
hdr->ver = ver;
hdr->seqid = g_seqid++;
hdr->cmdid = htons(cid);
hdr->transid = htons(g_transid++);
hdr->datalen = 0;
}
static struct calculated_checksum convert_ntoh(
FAR struct altcom_cmdhdr_s *hdr)
{
struct calculated_checksum checksum;
hdr->cmdid = ntohs(hdr->cmdid);
hdr->transid = ntohs(hdr->transid);
hdr->datalen = ntohs(hdr->datalen);
if (hdr->ver == ALTCOM_VER1)
{
/* Only V1 has footer */
hdr->v1_options = ntohs(hdr->v1_options);
hdr->v1_checksum = ntohs(hdr->v1_checksum);
struct altcom_cmdfooter_s *footer
= (struct altcom_cmdfooter_s *)&hdr->payload[hdr->datalen];
checksum.body_checksum = calc_checksum_v1(&hdr->payload[0],
sizeof(struct altcom_cmdfooter_s)-2 + hdr->datalen);
footer->reserve = ntohs(footer->reserve);
footer->checksum = ntohs(footer->checksum);
}
else
{
hdr->v4_hdr_cksum = ntohs(hdr->v4_hdr_cksum);
checksum.body_checksum = calc_checksum_v4(&hdr->payload[0],
hdr->datalen);
hdr->v4_data_cksum = ntohs(hdr->v4_data_cksum);
}
return checksum;
}
static int check_valid_pkt(FAR struct altcom_cmdhdr_s *hdr,
uint16_t body_checksum)
{
int valid_version = ALTCOM_VERX;
if (hdr->ver == ALTCOM_VER1)
{
struct altcom_cmdfooter_s *footer
= (struct altcom_cmdfooter_s *)&hdr->payload[hdr->datalen];
if (footer->checksum == body_checksum)
{
valid_version = ALTCOM_VER1;
}
else
{
m_err("[V1] cmdid: 0x%04x cmdlen: %d, "
"body checksum: 0x%04x/0x%04x\n",
hdr->cmdid, hdr->datalen,
footer->checksum, body_checksum);
}
}
else if(hdr->ver == ALTCOM_VER4)
{
if (hdr->v4_data_cksum == body_checksum)
{
valid_version = ALTCOM_VER4;
}
else
{
m_err("[V4] cmdid: 0x%04x cmdlen: %d, "
"body checksum: 0x%04x/0x%04x\n",
hdr->cmdid, hdr->datalen,
hdr->v4_data_cksum, body_checksum);
}
}
return valid_version;
}
static int check_valid_reply(FAR struct altcom_cmdhdr_s *hdr,
uint16_t body_checksum)
{
int valid_version = ALTCOM_VERX;
valid_version = check_valid_pkt(hdr, body_checksum);
if (valid_version == ALTCOM_VER1)
{
if ((hdr->cmdid == (ALTCOM_CMDID_POWER_ON_V1 | ALTCOM_CMDID_REPLY_BIT))
&& (hdr->datalen == ALTCOM_CMD_POWER_ON_REPLY_SIZE))
{
valid_version = ALTCOM_VER1;
}
else
{
valid_version = ALTCOM_VERX;
}
}
else if(valid_version == ALTCOM_VER4)
{
if ((hdr->cmdid == (ALTCOM_CMDID_POWER_ON_V4 | ALTCOM_CMDID_REPLY_BIT))
&& (hdr->datalen == ALTCOM_CMD_POWER_ON_REPLY_SIZE))
{
valid_version = ALTCOM_VER4;
}
else
{
valid_version = ALTCOM_VERX;
}
}
return valid_version;
}
static bool is_header_ok(FAR struct altcom_cmdhdr_s *hdr)
{
uint16_t checksum;
if (hdr->ver == ALTCOM_VER1)
{
checksum = calc_checksum_v1((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s) - sizeof(hdr->v1_checksum));
if (ntohs(hdr->v1_checksum) == checksum)
{
if (ntohs(hdr->datalen) + sizeof(struct altcom_cmdhdr_s)
+ sizeof(struct altcom_cmdfooter_s) <= ALTCOM_RX_PKT_SIZE_MAX)
{
return true;
}
else
{
m_err("[V1] Data length exceeding the buffer length: %d\n",
ntohs(hdr->datalen));
}
}
else
{
m_err("[V1] cmdid: 0x%04x cmdlen: %d, "
"header checksum: 0x%04x/0x%04x\n",
ntohs(hdr->cmdid), ntohs(hdr->datalen),
ntohs(hdr->v1_checksum), checksum);
}
}
else if (hdr->ver == ALTCOM_VER4)
{
checksum = calc_checksum_v4((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s) - sizeof(hdr->v4_hdr_cksum) -
sizeof(hdr->v4_data_cksum));
if (ntohs(hdr->v4_hdr_cksum) == checksum)
{
if (ntohs(hdr->datalen) + sizeof(struct altcom_cmdhdr_s) <=
ALTCOM_RX_PKT_SIZE_MAX)
{
return true;
}
else
{
m_err("[V4] Data length exceeding the buffer length: %d\n",
ntohs(hdr->datalen));
}
}
else
{
m_err("[V4] cmdid: 0x%04x cmdlen: %d, "
"header checksum: 0x%04x/0x%04x\n",
ntohs(hdr->cmdid), ntohs(hdr->datalen),
ntohs(hdr->v4_hdr_cksum), checksum);
}
}
return false;
}
/****************************************************************************
* Public Functions
****************************************************************************/
bool altcom_is_v1pkt_ok(struct altcom_cmdhdr_s *cmdhdr)
{
struct calculated_checksum checksum;
if (!is_header_ok(cmdhdr))
{
return false;
}
checksum = convert_ntoh(cmdhdr);
return (check_valid_reply(cmdhdr, checksum.body_checksum) == ALTCOM_VER1)
&& (cmdhdr->payload[0] == LTE_RESULT_OK);
}
bool altcom_is_v4pkt_ok(struct altcom_cmdhdr_s *cmdhdr)
{
struct calculated_checksum checksum;
if (!is_header_ok(cmdhdr))
{
return false;
}
checksum = convert_ntoh(cmdhdr);
return (check_valid_reply(cmdhdr, checksum.body_checksum) == ALTCOM_VER4)
&& (cmdhdr->payload[0] == LTE_RESULT_OK);
}
FAR void *altcom_make_poweron_cmd_v1(int *sz)
{
struct altcom_cmdhdr_s *hdr = (struct altcom_cmdhdr_s *)g_poweron_cmd;
struct altcom_cmdfooter_s *footer
= (struct altcom_cmdfooter_s *)&hdr->payload[0];
set_header_top(hdr, ALTCOM_VER1, ALTCOM_CMDID_POWER_ON_V1);
hdr->v1_options = htons(ALTCOM_CMDOPT_CHECKSUM_EN);
hdr->v1_checksum =
htons(calc_checksum_v1((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s) - 2));
footer->reserve = 0;
footer->checksum =
htons(calc_checksum_v1(&hdr->payload[0],
sizeof(struct altcom_cmdfooter_s) - 2));
/* No payload of this altcom command.
* So sending size is just header and footer size
*/
*sz = sizeof(struct altcom_cmdhdr_s) + sizeof(struct altcom_cmdfooter_s);
return g_poweron_cmd;
}
FAR void *altcom_make_poweron_cmd_v4(int *sz)
{
struct altcom_cmdhdr_s *hdr = (struct altcom_cmdhdr_s *)g_poweron_cmd;
set_header_top(hdr, ALTCOM_VER4, ALTCOM_CMDID_POWER_ON_V4);
hdr->v4_hdr_cksum = htons(calc_checksum_v4((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s)-4));
hdr->v4_data_cksum = htons(calc_checksum_v4(&hdr->payload[0], 0));
/* No payload of this altcom command. So sending size is just header size */
*sz = sizeof(struct altcom_cmdhdr_s);
return g_poweron_cmd;
}
int altcom_is_pkt_ok(FAR uint8_t *pkt, int sz)
{
struct calculated_checksum checksum;
int ver;
int ret = OK;
FAR struct altcom_cmdhdr_s *hdr = (FAR struct altcom_cmdhdr_s *)pkt;
int remlen;
if (!is_header_ok(hdr))
{
return -EPROTO;
}
remlen = get_pktlen(hdr->ver, ntohs(hdr->datalen)) - sz;
if (remlen > 0)
{
/* Cases in which fragmented packets are received. */
return remlen;
}
else if (remlen < 0)
{
/* The case where the received data length becomes
* larger than the payload length set in the header.
*/
return -EPROTO;
}
/* Whole packets are received. So check the validity of the packets. */
checksum = convert_ntoh((FAR struct altcom_cmdhdr_s *)pkt);
ver = check_valid_pkt((FAR struct altcom_cmdhdr_s *)pkt,
checksum.body_checksum);
if (ver == ALTCOM_VERX)
{
ret = -EPROTO;
}
return ret;
}
uint16_t altcom_make_header(FAR struct altcom_cmdhdr_s *hdr,
uint8_t ver, uint16_t cid, uint16_t sz)
{
uint16_t tid = g_transid;
hdr->magic = htonl(ALTCOM_HDR_MAGICNUMBER);
hdr->ver = ver;
hdr->seqid = g_seqid++;
hdr->cmdid = htons(cid);
hdr->transid = htons(g_transid++);
hdr->datalen = htons(sz);
if (ver == ALTCOM_VER1)
{
struct altcom_cmdfooter_s *footer
= (struct altcom_cmdfooter_s *)&hdr->payload[sz];
hdr->v1_options = htons(ALTCOM_CMDOPT_CHECKSUM_EN);
hdr->v1_checksum =
htons(calc_checksum_v1((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s) - 2));
footer->reserve = 0;
footer->checksum =
htons(calc_checksum_v1(&hdr->payload[0],
sizeof(struct altcom_cmdfooter_s) - 2 + sz));
}
else if (ver == ALTCOM_VER4)
{
hdr->v4_hdr_cksum = htons(calc_checksum_v4((uint8_t *)hdr,
sizeof(struct altcom_cmdhdr_s)-4));
hdr->v4_data_cksum = htons(calc_checksum_v4(&hdr->payload[0], sz));
}
else
{
DEBUGASSERT(0);
}
return tid;
}
Reference in New Issue View Git Blame Copy Permalink