struct vm_area_struct *add_vma(struct task_struct *task,
struct vm_operations_struct *vm_ops,
unsigned long size)
{
struct vm_area_struct *vma;
ulong* insert_vm_struct;
insert_vm_struct = (ulong*)kallsyms_lookup_name("insert_vm_struct");
if (insert_vm_struct == NULL) {
PR_WARN(NETMALLOC_WARN_SYMNOTFOUND);
goto error;
}
if (create_vm_area_struct(task, &vma, vm_ops, size) < 0)
goto error;
if (((insert_vm_prot *)insert_vm_struct)(current->mm, vma)) {
kfree(vma);
goto error;
}
return vma;
error:
return NULL;
}
int hisfc350_reg_erase(struct hisfc_host *host,
unsigned long long prt_size,
unsigned long long offset,
unsigned long long length,
int *state)
{
struct hisfc_spi *spi = host->spi;
if (offset + length > prt_size) {
PR_WARN("erase area out of range of mtd.\n");
return -EINVAL;
}
if ((unsigned int)offset & (host->erasesize-1)) {
PR_WARN("erase start address is not alignment.\n");
return -EINVAL;
}
if ((unsigned int)length & (host->erasesize-1)) {
PR_WARN("erase length is not alignment.\n");
return -EINVAL;
}
while (length) {
if (spi->chipsize <= offset) {
offset -= spi->chipsize;
spi++;
if (!spi->name)
PR_BUG("erase memory out of range.\n");
}
if (hisfc350_reg_erase_one_block(host, spi, offset)) {
mutex_unlock(&host->lock);
return -1;
}
offset += spi->erase->size;
length -= spi->erase->size;
}
*state = MTD_ERASE_DONE;
return 0;
}
int hisfc350_dma_write(struct hisfc_host *host,
unsigned long long prt_size,
unsigned int to,
unsigned int len,
unsigned int *retlen,
unsigned char *buf)
{
int num;
int result = -EIO;
unsigned char *ptr = (unsigned char *)buf;
struct hisfc_spi *spi = host->spi;
if ((unsigned long long)(to + len) > prt_size) {
PR_WARN("write data out of range.\n");
return -EINVAL;
}
*retlen = 0;
if (!len) {
PR_WARN("write length is 0.\n");
return 0;
}
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->write_enable(spi);
spi->driver->bus_prepare(spi, WRITE);
if (to & HISFC350_DMA_ALIGN_MASK) {
num = HISFC350_DMA_ALIGN_SIZE - (to & HISFC350_DMA_ALIGN_MASK);
if (num > len)
num = len;
while (to >= spi->chipsize) {
to -= spi->chipsize;
spi++;
if (!spi->name)
PR_BUG("write memory out of range.\n");
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->write_enable(spi);
spi->driver->bus_prepare(spi, WRITE);
}
memcpy(host->buffer, ptr, num);
hisfc350_dma_transfer(host, to,
(unsigned char *)host->dma_buffer, WRITE,
num, spi->chipselect);
to += num;
ptr += num;
len -= num;
}
while (len > 0) {
num = ((len >= HISFC350_DMA_MAX_SIZE)
? HISFC350_DMA_MAX_SIZE : len);
while (to >= spi->chipsize) {
to -= spi->chipsize;
spi++;
if (!spi->name)
PR_BUG("write memory out of range.\n");
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->write_enable(spi);
spi->driver->bus_prepare(spi, WRITE);
}
memcpy(host->buffer, ptr, num);
hisfc350_dma_transfer(host, to,
(unsigned char *)host->dma_buffer, WRITE,
num, spi->chipselect);
to += num;
ptr += num;
len -= num;
}
*retlen = (unsigned int)(ptr - buf);
result = 0;
fail:
return result;
}
int hisfc350_dma_read(struct hisfc_host *host,
unsigned long long prt_size,
unsigned int from,
unsigned int len,
unsigned int *retlen,
unsigned char *buf)
{
int num;
int result = -EIO;
unsigned char *ptr = buf;
struct hisfc_spi *spi = host->spi;
if ((unsigned long long)(from + len) > prt_size) {
PR_WARN("read area out of range.\n");
return -EINVAL;
}
*retlen = 0;
if (!len) {
PR_WARN("read length is 0.\n");
return 0;
}
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->bus_prepare(spi, READ);
if (from & HISFC350_DMA_ALIGN_MASK) {
num = HISFC350_DMA_ALIGN_SIZE - (from & HISFC350_DMA_ALIGN_MASK);
if (num > len)
num = len;
while (from >= spi->chipsize) {
from -= spi->chipsize;
spi++;
if (!spi->name)
PR_BUG("write memory out of range.\n");
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->bus_prepare(spi, READ);
}
hisfc350_dma_transfer(host, from,
(unsigned char *)host->dma_buffer, READ,
num, spi->chipselect);
memcpy(ptr, host->buffer, num);
from += num;
ptr += num;
len -= num;
}
while (len > 0) {
while (from >= spi->chipsize) {
from -= spi->chipsize;
spi++;
if (!spi->name)
PR_BUG("read memory out of range.\n");
if (spi->driver->wait_ready(spi))
goto fail;
spi->driver->bus_prepare(spi, READ);
}
num = ((from + len) >= spi->chipsize)
? (spi->chipsize - from) : len;
while (num >= HISFC350_DMA_MAX_SIZE) {
hisfc350_dma_transfer(host, from,
(unsigned char *)host->dma_buffer, READ,
HISFC350_DMA_MAX_SIZE, spi->chipselect);
memcpy(ptr, host->buffer, HISFC350_DMA_MAX_SIZE);
ptr += HISFC350_DMA_MAX_SIZE;
from += HISFC350_DMA_MAX_SIZE;
len -= HISFC350_DMA_MAX_SIZE;
num -= HISFC350_DMA_MAX_SIZE;
}
if (num) {
hisfc350_dma_transfer(host, from,
(unsigned char *)host->dma_buffer, READ,
num, spi->chipselect);
memcpy(ptr, host->buffer, num);
from += num;
ptr += num;
len -= num;
}
}
result = 0;
*retlen = (unsigned int)(ptr - buf);
fail:
return result;
}
int __nfct_msg_set_tuple(nfct_msg *m, int type, const conn_tuple *t)
{
conn_entry *e = (conn_entry *)m->entry;
nfct_msg_ctl *ctl = (nfct_msg_ctl *)m;
struct nlattr **nla = e->nla;
struct nlattr *tuple, *ip, *proto;
__u16 l3 = t->src.l3num;
__u8 l4 = t->dst.protonum;
size_t sz, ip_sz = 0, proto_sz = 0;
if(nla[type] || (l3 != AF_INET && l3 != AF_INET6))
return -1;
if(l3 == AF_INET) {
ip_sz += nla_total_size(sizeof(struct in_addr));
ip_sz += nla_total_size(sizeof(struct in_addr));
}else {
ip_sz += nla_total_size(sizeof(struct in6_addr));
ip_sz += nla_total_size(sizeof(struct in6_addr));
}
proto_sz += nla_total_size(sizeof(__u8));
switch(l4) {
case IPPROTO_TCP:
case IPPROTO_UDP:
case IPPROTO_UDPLITE:
case IPPROTO_SCTP:
proto_sz += nla_total_size(sizeof(__u16));
proto_sz += nla_total_size(sizeof(__u16));
break;
case IPPROTO_ICMP:
case IPPROTO_ICMPV6:
proto_sz += nla_total_size(sizeof(__u8)) * 2;
proto_sz += nla_total_size(sizeof(__u16));
break;
default:
PR_WARN("unsupported l4 %d while settings tuple", l4);
return -1;
}
sz = nla_total_size(nla_total_size(ip_sz) + nla_total_size(proto_sz));
if(sz < msg_free_space(ctl)) {
tuple = nla_nested_start(&ctl->ctx, type);
ip = nla_nested_start(&ctl->ctx, CTA_TUPLE_IP);
if(l3 == AF_INET) {
nla_put_be32(&ctl->ctx, CTA_IP_V4_SRC, t->src.u3.ip);
nla_put_be32(&ctl->ctx, CTA_IP_V4_DST, t->dst.u3.ip);
}else {
nla_put_blob(&ctl->ctx, CTA_IP_V6_SRC, &t->src.u3.in6, sizeof(t->src.u3.in6));
nla_put_blob(&ctl->ctx, CTA_IP_V6_DST, &t->dst.u3.in6, sizeof(t->dst.u3.in6));
}
nla_nested_end(ip, ctl->ctx);
proto = nla_nested_start(&ctl->ctx, CTA_TUPLE_PROTO);
nla_put_u8(&ctl->ctx, CTA_PROTO_NUM, l4);
switch(l4) {
case IPPROTO_TCP:
case IPPROTO_UDP:
case IPPROTO_UDPLITE:
case IPPROTO_SCTP:
nla_put_be16(&ctl->ctx, CTA_PROTO_SRC_PORT, t->src.u.tcp.port);
nla_put_be16(&ctl->ctx, CTA_PROTO_DST_PORT, t->dst.u.tcp.port);
break;
case IPPROTO_ICMP:
nla_put_be16(&ctl->ctx, CTA_PROTO_ICMP_ID, t->src.u.icmp.id);
nla_put_u8(&ctl->ctx, CTA_PROTO_ICMP_TYPE, t->dst.u.icmp.type);
nla_put_u8(&ctl->ctx, CTA_PROTO_ICMP_CODE, t->dst.u.icmp.code);
break;
case IPPROTO_ICMPV6:
nla_put_be16(&ctl->ctx, CTA_PROTO_ICMPV6_ID, t->src.u.icmp.id);
nla_put_u8(&ctl->ctx, CTA_PROTO_ICMPV6_TYPE, t->dst.u.icmp.type);
nla_put_u8(&ctl->ctx, CTA_PROTO_ICMPV6_CODE, t->dst.u.icmp.code);
break;
default:
PR_WARN("unsupported l4 %d while settings tuple", l4);
return -1;
}
nla_nested_end(proto, ctl->ctx);
nla_nested_end(tuple, ctl->ctx);
nla[type] = tuple;
return 0;
}
return -1;
}
nfct_t *nfct_create(ginkgo_ctx *ctx, int grps, nfct_notify cb, void *ud)
{
nfct_t *ct;
char name[20];
ginkgo_src src = {
.name = name,
.fd = -1,
.rd = NULL,
.wr = NULL,
.pars = nl_parse,
.resp = nfct_response,
.hand = nfct_handler,
.ud = NULL,
};
int fd;
if(! ctx) return NULL;
if((fd = nl_open(NETLINK_NETFILTER, grps)) < 0)
return NULL;
if(! instantiate(ct)) {
close(fd);
return NULL;
}
ct->ginkgo = ctx;
ct->nlfd = fd;
ct->nlgrps = grps;
ct->nlseq = 0;
ct->cb = cb;
ct->ud = ud;
src.fd = fd;
src.ud = ct;
sprintf(name, "nfct-%d", fd);
if(ginkgo_src_register(ctx, &src, &ct->id, 0) < 0) {
close(fd);
free(ct);
return NULL;
}
return ct;
}
void nfct_destroy(nfct_t *ct)
{
if(ct) {
ginkgo_src_deregister(ct->ginkgo, ct->id, 1, 1);
close(ct->nlfd);
free(ct);
}
}
int nfct_conn_get_counter(const conn_entry *e, conn_counter *counter)
{
const struct nlattr *orig = e->nla[CTA_COUNTERS_ORIG];
const struct nlattr *rep = e->nla[CTA_COUNTERS_REPLY];
struct nlattr *arr[CTA_COUNTERS_MAX + 1];
if(orig && rep) {
nla_parse_nested(arr, CTA_COUNTERS_MAX, orig);
if(! arr[CTA_COUNTERS_PACKETS] || ! arr[CTA_COUNTERS_BYTES])
return -1;
counter->orig_pkts = be64toh(nla_get_be64(arr[CTA_COUNTERS_PACKETS]));
counter->orig_bytes = be64toh(nla_get_be64(arr[CTA_COUNTERS_BYTES]));
nla_parse_nested(arr, CTA_COUNTERS_MAX, rep);
if(! arr[CTA_COUNTERS_PACKETS] || ! arr[CTA_COUNTERS_BYTES])
return -1;
counter->rep_pkts = be64toh(nla_get_be64(arr[CTA_COUNTERS_PACKETS]));
counter->rep_bytes = be64toh(nla_get_be64(arr[CTA_COUNTERS_BYTES]));
return 0;
}
return -1;
}
int nfct_conn_get_tcpinfo(const conn_entry *e, conn_tcpinfo *info)
{
const struct nlattr *proto = e->nla[CTA_PROTOINFO];
const struct nlattr *tcp;
struct nlattr *arr[CTA_PROTOINFO_TCP_MAX + 1];
if(proto) {
tcp = (const struct nlattr *)nla_data(proto);
if(nla_type(tcp) != CTA_PROTOINFO_TCP)
return -1;
nla_parse_nested(arr, CTA_PROTOINFO_TCP_MAX, tcp);
if(arr[CTA_PROTOINFO_TCP_STATE]
|| arr[CTA_PROTOINFO_TCP_WSCALE_ORIGINAL]
|| arr[CTA_PROTOINFO_TCP_WSCALE_REPLY]
|| arr[CTA_PROTOINFO_TCP_FLAGS_ORIGINAL]
|| arr[CTA_PROTOINFO_TCP_FLAGS_REPLY]) {
info->state = nla_get_u8(arr[CTA_PROTOINFO_TCP_STATE]);
info->wscale_orig = nla_get_u8(arr[CTA_PROTOINFO_TCP_WSCALE_ORIGINAL]);
info->wscale_rep = nla_get_u8(arr[CTA_PROTOINFO_TCP_WSCALE_REPLY]);
nla_get_mem(arr[CTA_PROTOINFO_TCP_FLAGS_ORIGINAL], &info->flags_orig, sizeof(info->flags_orig));
nla_get_mem(arr[CTA_PROTOINFO_TCP_FLAGS_REPLY], &info->flags_rep, sizeof(info->flags_rep));
return 0;
}
}
return -1;
}
int nfct_conn_get_sctpinfo(const conn_entry *e, conn_sctpinfo *info)
{
const struct nlattr *proto = e->nla[CTA_PROTOINFO];
const struct nlattr *sctp;
struct nlattr *arr[CTA_PROTOINFO_SCTP_MAX + 1];
if(proto) {
sctp = (const struct nlattr *)nla_data(proto);
if(nla_type(sctp) != CTA_PROTOINFO_SCTP)
return -1;
nla_parse_nested(arr, CTA_PROTOINFO_SCTP_MAX, sctp);
if(! arr[CTA_PROTOINFO_SCTP_STATE]
|| ! arr[CTA_PROTOINFO_SCTP_VTAG_ORIGINAL]
|| ! arr[CTA_PROTOINFO_SCTP_VTAG_REPLY]) {
info->state = nla_get_u8(arr[CTA_PROTOINFO_SCTP_STATE]);
info->vtag_orig = ntohl(nla_get_u8(arr[CTA_PROTOINFO_SCTP_VTAG_ORIGINAL]));
info->vtag_rep = ntohl(nla_get_u8(arr[CTA_PROTOINFO_SCTP_VTAG_REPLY]));
return 0;
}
}
return -1;
}
int __nfct_conn_get_tuple(const conn_entry *e, int type, conn_tuple *tuple)
{
struct nlattr *arr[CTA_TUPLE_MAX + 1];
struct nlattr *ip[CTA_IP_MAX + 1];
struct nlattr *proto[CTA_PROTO_MAX + 1];
if(! e->nla[type])
return -1;
memset(tuple, 0, sizeof(*tuple));
nla_parse_nested(arr, CTA_TUPLE_MAX, e->nla[type]);
if(! arr[CTA_TUPLE_IP] || ! arr[CTA_TUPLE_PROTO]) {
PR_WARN("unexpected NULL tuple IP or proto");
return-1;
}
nla_parse_nested(ip, CTA_IP_MAX, arr[CTA_TUPLE_IP]);
tuple->src.l3num = e->l3num;
if(e->l3num == AF_INET) {
if(! ip[CTA_IP_V4_SRC] || ! ip[CTA_IP_V4_DST])
return -1;
tuple->src.u3.ip = nla_get_be32(ip[CTA_IP_V4_SRC]);
tuple->dst.u3.ip = nla_get_be32(ip[CTA_IP_V4_DST]);
}else if(e->l3num == AF_INET6) {
if(! ip[CTA_IP_V6_SRC] || ! ip[CTA_IP_V6_DST])
return -1;
nla_get_mem(ip[CTA_IP_V6_SRC], &tuple->src.u3.in6, sizeof(tuple->src.u3.in6));
nla_get_mem(ip[CTA_IP_V6_DST], &tuple->dst.u3.in6, sizeof(tuple->dst.u3.in6));
}else {
PR_WARN("unexpected l3num while parsing tuple:%d", e->l3num);
return -1;
}
nla_parse_nested(proto, CTA_PROTO_MAX, arr[CTA_TUPLE_PROTO]);
if(! proto[CTA_PROTO_NUM]) {
PR_WARN("unexpected NULL proto num while parsing tuple");
return -1;
}
tuple->dst.protonum = nla_get_u8(proto[CTA_PROTO_NUM]);
switch(tuple->dst.protonum) {
case IPPROTO_TCP:
case IPPROTO_UDP:
case IPPROTO_UDPLITE:
if(! proto[CTA_PROTO_SRC_PORT] || ! proto[CTA_PROTO_DST_PORT]) {
PR_WARN("unexpected NULL port while parsing tuple");
return -1;
}
tuple->src.u.tcp.port = nla_get_be16(proto[CTA_PROTO_SRC_PORT]);
tuple->dst.u.tcp.port = nla_get_be16(proto[CTA_PROTO_DST_PORT]);
break;
case IPPROTO_ICMP:
if(! proto[CTA_PROTO_ICMP_ID] || ! proto[CTA_PROTO_ICMP_TYPE] || ! proto[CTA_PROTO_ICMP_CODE]) {
PR_WARN("unexpected NULL icmp attr while parsing tuple");
return -1;
}
tuple->src.u.icmp.id = nla_get_be16(proto[CTA_PROTO_ICMP_ID]);
tuple->dst.u.icmp.type = nla_get_u8(proto[CTA_PROTO_ICMP_TYPE]);
tuple->dst.u.icmp.code = nla_get_u8(proto[CTA_PROTO_ICMP_CODE]);
break;
case IPPROTO_ICMPV6:
if(! proto[CTA_PROTO_ICMPV6_ID]
|| ! proto[CTA_PROTO_ICMPV6_TYPE]
|| ! proto[CTA_PROTO_ICMPV6_CODE]) {
PR_WARN("unexpected NULL icmpv6 attr while parsing tuple");
return -1;
}
tuple->src.u.icmp.id = nla_get_be16(proto[CTA_PROTO_ICMPV6_ID]);
tuple->dst.u.icmp.type = nla_get_u8(proto[CTA_PROTO_ICMPV6_TYPE]);
tuple->dst.u.icmp.code = nla_get_u8(proto[CTA_PROTO_ICMPV6_CODE]);
break;
default:
PR_INFO("unsupported proto %d while parsing tuple", tuple->dst.protonum);
return -1;
}
return 0;
}
/* fixme: nat seq adj */
nfct_msg *nfct_msg_new(__u8 l3num, int flags)
{
ginkgo_msg *msg;
struct nlmsghdr *nlh;
struct nfgenmsg *nfmsg;
nfct_msg_ctl *ctl;
conn_entry *e;
if((msg = ginkgo_new_msg(0, NFCT_MSG_GOOD_SIZE))) {
if(! instantiate(e)) {
free(msg);
return NULL;
}
nlh = NL_HEADER(msg);
BZERO(nlh);
nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
if(flags & NFCT_F_CREATE)
nlh->nlmsg_flags |= NLM_F_CREATE;
if(flags & NFCT_F_EXCL)
nlh->nlmsg_flags |= NLM_F_EXCL;
if(flags & NFCT_F_DUMP)
nlh->nlmsg_flags |= NLM_F_DUMP;
nfmsg = (struct nfgenmsg *)NLMSG_DATA(nlh);
nfmsg->nfgen_family = l3num;
nfmsg->version = NFNETLINK_V0;
nfmsg->res_id = 0;
BZERO(e);
e->l3num = l3num;
ctl = msg_ctl(msg);
BZERO(ctl);
ctl->base.entry = e;
ctl->ctx = (char *)nfmsg + NLMSG_ALIGN(sizeof(struct nfgenmsg));
ctl->sz = NFCT_MSG_GOOD_SIZE;
return (nfct_msg *)ctl;
}
return NULL;
}
