/*
ret < 0 : error
ret == 0 : no data
ret > 0 : valid data
*/
static int dpram_ipc_recv_data(struct dpram_link_device *dpld, int dev)
{
struct link_device *ld = &dpld->ld;
struct dpram_rxb *rxb;
u8 __iomem *src = get_rx_buff(dpld, dev);
u32 qsize = get_rx_buff_size(dpld, dev);
u32 in = get_rx_head(dpld, dev);
u32 out = get_rx_tail(dpld, dev);
u32 rcvd = 0;
struct mif_irq_map map;
if (in == out)
return 0;
if (dev == IPC_FMT) {
set_dpram_map(dpld, &map);
mif_irq_log(ld->mc->msd, map, "ipc_recv", sizeof("ipc_recv"));
}
/* Get data length in DPRAM*/
rcvd = (in > out) ? (in - out) : (qsize - out + in);
mif_debug("%s: %s qsize[%u] in[%u] out[%u] rcvd[%u]\n",
ld->name, get_dev_name(dev), qsize, in, out, rcvd);
/* Check each queue */
if (!dpram_circ_valid(qsize, in, out)) {
mif_err("%s: ERR! %s_RXQ invalid (size:%d in:%d out:%d)\n",
ld->name, get_dev_name(dev), qsize, in, out);
#if 0
set_rx_head(dpld, dev, 0);
set_rx_tail(dpld, dev, 0);
#else
dpram_trigger_force_cp_crash(dpld);
#endif
return -EINVAL;
}
/* Allocate an rxb */
rxb = rxbq_get_free_rxb(&dpld->rxbq[dev]);
if (!rxb) {
mif_info("%s: ERR! %s rxbq_get_free_rxb fail\n",
ld->name, get_dev_name(dev));
return -ENOMEM;
}
/* Read data from each DPRAM buffer */
dpram_ipc_read(dpld, dev, rxb_put(rxb, rcvd), src, out, rcvd, qsize);
/* Calculate and set new out */
out += rcvd;
if (out >= qsize)
out -= qsize;
set_rx_tail(dpld, dev, out);
return rcvd;
}
static void dpram_send_ipc(struct link_device *ld, int dev,
struct io_device *iod, struct sk_buff *skb)
{
struct dpram_link_device *dpld = to_dpram_link_device(ld);
struct sk_buff_head *txq = ld->skb_txq[dev];
int ret;
u16 mask;
skb_queue_tail(txq, skb);
if (txq->qlen > 1024) {
mif_debug("%s: %s txq->qlen %d > 1024\n",
ld->name, get_dev_name(dev), txq->qlen);
}
if (dpld->dp_type == CP_IDPRAM) {
if (dpram_wake_up(dpld) < 0) {
trigger_force_cp_crash(dpld);
return;
}
}
if (!dpram_ipc_active(dpld))
goto exit;
if (atomic_read(&dpld->res_required[dev]) > 0) {
mif_debug("%s: %s_TXQ is full\n", ld->name, get_dev_name(dev));
goto exit;
}
ret = dpram_try_ipc_tx(dpld, dev);
if (ret > 0) {
mask = get_mask_send(dpld, dev);
send_intr(dpld, INT_NON_CMD(mask));
} else if (ret == -ENOSPC) {
mask = get_mask_req_ack(dpld, dev);
send_intr(dpld, INT_NON_CMD(mask));
mif_info("%s: Send REQ_ACK 0x%04X\n", ld->name, mask);
} else {
mif_info("%s: dpram_try_ipc_tx fail (err %d)\n", ld->name, ret);
}
exit:
if (dpld->dp_type == CP_IDPRAM)
dpram_allow_sleep(dpld);
}
bool legacy_smart_device::open()
{
m_fd = ::deviceopen(get_dev_name(), const_cast<char*>(m_mode));
if (m_fd < 0) {
set_err((errno==ENOENT || errno==ENOTDIR) ? ENODEV : errno);
return false;
}
return true;
}
int dm_format_dev(char *buf, int bufsize, uint32_t dev_major,
uint32_t dev_minor)
{
int r;
uint32_t major, dm_major;
char *name;
mode_t mode;
dev_t dev;
size_t val_len,i;
struct kinfo_drivers *kd;
mode = 0;
nbsd_get_dm_major(&dm_major, DM_BLOCK_MAJOR);
if (bufsize < 8)
return 0;
if (sysctlbyname("kern.drivers",NULL,&val_len,NULL,0) < 0) {
printf("sysctlbyname failed");
return 0;
}
if ((kd = malloc (val_len)) == NULL){
printf("malloc kd info error\n");
return 0;
}
if (sysctlbyname("kern.drivers", kd, &val_len, NULL, 0) < 0) {
printf("sysctlbyname failed kd");
return 0;
}
for (i = 0, val_len /= sizeof(*kd); i < val_len; i++){
if (kd[i].d_cmajor == dev_major) {
major = kd[i].d_bmajor;
break;
}
}
dev = MKDEV(major,dev_minor);
mode |= S_IFBLK;
if ((name = devname(dev,mode)) == NULL)
name = get_dev_name(kd[i].d_name, major, dev_minor);
r = snprintf(buf, (size_t) bufsize, "/dev/%s",name);
free(kd);
if (r < 0 || r > bufsize - 1 || name == NULL)
return 0;
return 1;
}
TunTap(const int fd, const std::string &dev_name):
gras::HierBlock("GREX TunTap"),
_fd(fd),
_dev_name(dev_name)
{
//helpful user message
std::cout << boost::format(
"Allocated virtual ethernet interface: %s\n"
"You must now use ifconfig to set its IP address. E.g.,\n"
" $ sudo ifconfig %s 192.168.200.1\n"
"Be sure to use a different address in the same subnet for each machine.\n"
) % get_dev_name() % get_dev_name() << std::endl;
//make the blocks
_datagram_to_fildes = boost::make_shared<Datagram2Filedes>(_fd);
_filedes_to_datagram = boost::make_shared<Filedes2Datagram>(_fd);
//connect
this->connect(*this, 0, _datagram_to_fildes, 0);
this->connect(_filedes_to_datagram, 0, *this, 0);
}
/* Get data size in the RXQ as well as in & out pointers */
static inline int dpram_get_rxq_rcvd(struct dpram_link_device *dpld, int dev,
u32 qsize, u32 *in, u32 *out)
{
struct link_device *ld = &dpld->ld;
int cnt = 3;
u32 head;
u32 tail;
u32 rcvd;
do {
head = get_rx_head(dpld, dev);
tail = get_rx_tail(dpld, dev);
if (head == tail) {
*in = head;
*out = tail;
return 0;
}
rcvd = (head > tail) ? (head - tail) : (qsize - tail + head);
mif_debug("%s: %s_RXQ qsize[%u] in[%u] out[%u] rcvd[%u]\n",
ld->name, get_dev_name(dev), qsize, head, tail, rcvd);
if (dpram_circ_valid(qsize, head, tail)) {
*in = head;
*out = tail;
return rcvd;
}
mif_info("%s: CAUTION! <%pf> "
"%s_RXQ invalid (size:%d in:%d out:%d)\n",
ld->name, __builtin_return_address(0),
get_dev_name(dev), qsize, head, tail);
udelay(100);
} while (cnt--);
*in = 0;
*out = 0;
return -EINVAL;
}
static void non_command_handler(struct dpram_link_device *dpld, u16 intr)
{
struct link_device *ld = &dpld->ld;
int i = 0;
int ret = 0;
u16 tx_mask = 0;
if (!dpram_ipc_active(dpld))
return;
/* Read data from DPRAM */
for (i = 0; i < dpld->max_ipc_dev; i++) {
if (dpld->use_skb)
ret = dpram_ipc_recv_data_with_skb(dpld, i);
else
ret = dpram_ipc_recv_data_with_rxb(dpld, i);
if (ret < 0)
dpram_reset_rx_circ(dpld, i);
/* Check and process REQ_ACK (at this time, in == out) */
if (intr & get_mask_req_ack(dpld, i)) {
mif_debug("%s: send %s_RES_ACK\n",
ld->name, get_dev_name(i));
tx_mask |= get_mask_res_ack(dpld, i);
}
}
if (!dpld->use_skb) {
/* Schedule soft IRQ for RX */
tasklet_hi_schedule(&dpld->rx_tsk);
}
/* Try TX via DPRAM */
for (i = 0; i < dpld->max_ipc_dev; i++) {
if (atomic_read(&dpld->res_required[i]) > 0) {
ret = dpram_try_ipc_tx(dpld, i);
if (ret > 0) {
atomic_set(&dpld->res_required[i], 0);
tx_mask |= get_mask_send(dpld, i);
} else if (ret == -ENOSPC) {
tx_mask |= get_mask_req_ack(dpld, i);
}
}
}
if (tx_mask) {
send_intr(dpld, INT_NON_CMD(tx_mask));
mif_debug("%s: send intr 0x%04X\n", ld->name, tx_mask);
}
}
UWORD16 add_p2p_device_info_attr(UWORD8 *ie_ptr, UWORD16 index)
{
UWORD8* dev_name = 0;
UWORD16 attr_len = 0;
UWORD16 wps_config = 0;
UWORD16 attr_start_offset = index;
TROUT_FUNC_ENTER;
ie_ptr[index] = P2P_DEVICE_INFO; /* Attribute ID */
index += P2P_ATTR_HDR_LEN;
/* Copy the device address */
mac_addr_cpy((ie_ptr + index), mget_StationID());
index += 6;
/* Copy the config method */
wps_config = get_wps_config_method();
PUT_U16_BE((ie_ptr + index), wps_config);
index += 2;
/* Copy the primary device type */
/* Note : The get_prim_dev_type() returns the pointer to the start of the*/
/* memory location which stores the length of the primary device type in */
/* the first 2 bytes and the attribute in the next 8 bytes */
memcpy((ie_ptr + index), get_prim_dev_type() + 2, 8);
index += 8;
/* Number of secondary device type */
ie_ptr[index++] = 0; /* or a funtion get_num_secondary_device() */
/* assuming no seconday device */
/* Copy the device name */
PUT_U16_BE((ie_ptr + index), WPS_ATTR_DEV_NAME_P2P);
index += 2;
dev_name = get_dev_name();
PUT_U16_BE((ie_ptr + index), dev_name[0]);
index += 2;
memcpy((ie_ptr + index), &dev_name[1], dev_name[0]);
index += dev_name[0];
/* Update the length field of the attribute */
attr_len = index - (attr_start_offset + P2P_ATTR_HDR_LEN);
ie_ptr[attr_start_offset + 1] = attr_len & 0x00FF;
ie_ptr[attr_start_offset + 2] = attr_len >> 8;
TROUT_FUNC_EXIT;
return(attr_len + P2P_ATTR_HDR_LEN);
}
/* Get free space in the TXQ as well as in & out pointers */
static inline int dpram_get_txq_space(struct dpram_link_device *dpld, int dev,
u32 qsize, u32 *in, u32 *out)
{
struct link_device *ld = &dpld->ld;
int cnt = 3;
u32 head;
u32 tail;
int space;
do {
head = get_tx_head(dpld, dev);
tail = get_tx_tail(dpld, dev);
space = (head < tail) ? (tail - head - 1) :
(qsize + tail - head - 1);
mif_debug("%s: %s_TXQ qsize[%u] in[%u] out[%u] space[%u]\n",
ld->name, get_dev_name(dev), qsize, head, tail, space);
if (dpram_circ_valid(qsize, head, tail)) {
*in = head;
*out = tail;
return space;
}
mif_info("%s: CAUTION! <%pf> "
"%s_TXQ invalid (size:%d in:%d out:%d)\n",
ld->name, __builtin_return_address(0),
get_dev_name(dev), qsize, head, tail);
udelay(100);
} while (cnt--);
*in = 0;
*out = 0;
return -EINVAL;
}
/**
* print_ipc_trace
* @ld: pointer to an instance of link_device structure
* @dev: IPC device (IPC_FMT, IPC_RAW, etc.)
* @stat: pointer to an instance of circ_status structure
* @ts: pointer to an instance of timespec structure
* @buff: start address of a buffer into which RX IPC messages were copied
* @rcvd: size of data in the buffer
*
* Prints IPC messages in a local memory buffer to a kernel log.
*/
void print_ipc_trace(struct link_device *ld, int dev, struct circ_status *stat,
struct timespec *ts, u8 *buff, u32 rcvd)
{
struct utc_time utc;
ts2utc(ts, &utc);
pr_info("%s: [%d-%02d-%02d %02d:%02d:%02d.%03d] "
"%s %s_RXQ {IN:%u OUT:%u LEN:%d}\n",
MIF_TAG, utc.year, utc.mon, utc.day, utc.hour, utc.min, utc.sec,
utc.msec, ld->name, get_dev_name(dev), stat->in, stat->out,
stat->size);
mif_print_dump(buff, rcvd, 4);
}
/**
* print_circ_status
* @ld: pointer to an instance of link_device structure
* @dev: IPC device (IPC_FMT, IPC_RAW, etc.)
* @mst: pointer to an instance of mem_status structure
*
* Prints a snapshot of the status of a memory
*/
void print_circ_status(struct link_device *ld, int dev, struct mem_status *mst)
{
struct utc_time utc;
int us = ns2us(mst->ts.tv_nsec);
if (dev > IPC_RAW)
return;
ts2utc(&mst->ts, &utc);
pr_info("%s: %s: [%02d:%02d:%02d.%06d] [%s] %s | "
"TXQ{in:%u out:%u} RXQ{in:%u out:%u} | INTR{0x%02X}\n",
MIF_TAG, ld->name, utc.hour, utc.min, utc.sec, us,
get_dir_str(mst->dir), get_dev_name(dev),
mst->head[dev][TX], mst->tail[dev][TX],
mst->head[dev][RX], mst->tail[dev][RX], mst->int2ap);
}
static int dpram_try_ipc_tx(struct dpram_link_device *dpld, int dev)
{
struct link_device *ld = &dpld->ld;
struct sk_buff_head *txq = ld->skb_txq[dev];
struct sk_buff *skb;
unsigned long int flags;
u32 qsize = get_tx_buff_size(dpld, dev);
u32 in;
u32 out;
int space;
int copied = 0;
spin_lock_irqsave(&dpld->tx_lock[dev], flags);
while (1) {
space = dpram_get_txq_space(dpld, dev, qsize, &in, &out);
if (unlikely(space < 0)) {
spin_unlock_irqrestore(&dpld->tx_lock[dev], flags);
dpram_reset_tx_circ(dpld, dev);
return space;
}
skb = skb_dequeue(txq);
if (unlikely(!skb))
break;
if (unlikely(space < skb->len)) {
atomic_set(&dpld->res_required[dev], 1);
skb_queue_head(txq, skb);
spin_unlock_irqrestore(&dpld->tx_lock[dev], flags);
mif_info("%s: %s "
"qsize[%u] in[%u] out[%u] free[%u] < len[%u]\n",
ld->name, get_dev_name(dev),
qsize, in, out, space, skb->len);
return -ENOSPC;
}
/* TX if there is enough room in the queue */
dpram_ipc_write(dpld, dev, qsize, in, out, skb);
copied += skb->len;
dev_kfree_skb_any(skb);
}
spin_unlock_irqrestore(&dpld->tx_lock[dev], flags);
return copied;
}
static void dpram_trigger_crash(struct dpram_link_device *dpld)
{
struct link_device *ld = &dpld->ld;
struct io_device *iod;
int i;
for (i = 0; i < dpld->max_ipc_dev; i++) {
mif_info("%s: purging %s_skb_txq\b", ld->name, get_dev_name(i));
skb_queue_purge(ld->skb_txq[i]);
}
iod = link_get_iod_with_format(ld, IPC_FMT);
iod->modem_state_changed(iod, STATE_CRASH_EXIT);
iod = link_get_iod_with_format(ld, IPC_BOOT);
iod->modem_state_changed(iod, STATE_CRASH_EXIT);
iod = link_get_iod_with_channel(ld, PS_DATA_CH_0);
if (iod)
iodevs_for_each(iod->msd, iodev_netif_stop, 0);
}
/*
ret < 0 : error
ret == 0 : no data
ret > 0 : valid data
*/
static int dpram_ipc_recv_data_with_rxb(struct dpram_link_device *dpld, int dev)
{
struct link_device *ld = &dpld->ld;
struct dpram_rxb *rxb;
u8 __iomem *src = get_rx_buff(dpld, dev);
u32 qsize = get_rx_buff_size(dpld, dev);
u32 in;
u32 out;
u32 rcvd;
struct mif_irq_map map;
rcvd = dpram_get_rxq_rcvd(dpld, dev, qsize, &in, &out);
if (rcvd <= 0)
return rcvd;
if (dev == IPC_FMT) {
set_dpram_map(dpld, &map);
mif_irq_log(ld->mc->msd, map, "ipc_recv", sizeof("ipc_recv"));
}
/* Allocate an rxb */
rxb = rxbq_get_free_rxb(&dpld->rxbq[dev]);
if (!rxb) {
mif_info("%s: ERR! %s rxbq_get_free_rxb fail\n",
ld->name, get_dev_name(dev));
return -ENOMEM;
}
/* Read data from each DPRAM buffer */
dpram_ipc_read(dpld, dev, rxb_put(rxb, rcvd), src, out, rcvd, qsize);
/* Calculate and set new out */
out += rcvd;
if (out >= qsize)
out -= qsize;
set_rx_tail(dpld, dev, out);
return rcvd;
}
struct audio_device *device_register(DBusConnection *conn,
const char *path, const bdaddr_t *bda, const bdaddr_t *src)
{
struct audio_device *dev;
int dev_id = 0;
if (!conn || !path)
return NULL;
dev = g_new0(struct audio_device, 1);
/* FIXME just to maintain compatibility */
dev->adapter_path = g_strdup_printf("/org/bluez/hci%d", dev_id);
if (!dev->adapter_path) {
device_free(dev);
return NULL;
}
if (!g_dbus_register_interface(conn, path,
AUDIO_DEVICE_INTERFACE,
device_methods, NULL, NULL,
dev, device_unregister)) {
error("Failed to register %s interface to %s",
AUDIO_DEVICE_INTERFACE, path);
device_free(dev);
return NULL;
}
dev->name = get_dev_name(conn, src, bda);
dev->path = g_strdup(path);
bacpy(&dev->dst, bda);
bacpy(&dev->src, src);
bacpy(&dev->store, src);
dev->conn = dbus_connection_ref(conn);
return dev;
}
static inline void set_rx_tail(struct dpram_link_device *dpld, int id, u32 tail)
{
int cnt = 3;
u32 val = 0;
iowrite16((u16)tail, dpld->dev[id]->rxq.tail);
do {
/* Check tail value written */
val = ioread16(dpld->dev[id]->rxq.tail);
if (val == tail)
return;
mif_err("ERR: %s rxq.tail(%d) != tail(%d)\n",
get_dev_name(id), val, tail);
udelay(100);
/* Write tail value again */
iowrite16((u16)tail, dpld->dev[id]->rxq.tail);
} while (cnt--);
dpram_trigger_force_cp_crash(dpld);
}
static inline void set_tx_head(struct dpram_link_device *dpld, int id, u32 head)
{
int cnt = 3;
u32 val = 0;
iowrite16((u16)head, dpld->dev[id]->txq.head);
do {
/* Check head value written */
val = ioread16(dpld->dev[id]->txq.head);
if (likely(val == head))
return;
mif_err("ERR: %s txq.head(%d) != head(%d)\n",
get_dev_name(id), val, head);
udelay(100);
/* Write head value again */
iowrite16((u16)head, dpld->dev[id]->txq.head);
} while (cnt--);
dpram_trigger_force_cp_crash(dpld);
}
/* get free space in the TXQ as well as in & out pointers */
static inline int get_txq_space(struct dpram_link_device *dpld, int dev,
u32 qsize, u32 *in, u32 *out)
{
struct link_device *ld = &dpld->ld;
int cnt = 3;
int space;
do {
*in = get_tx_head(dpld, dev);
*out = get_tx_tail(dpld, dev);
if (dpram_circ_valid(qsize, *in, *out)) {
space = (*in < *out) ? (*out - *in - 1) :
(qsize + *out - *in - 1);
return space;
}
mif_info("%s: CAUTION! <%pf> "
"%s_TXQ invalid (size:%d in:%d out:%d)\n",
ld->name, __builtin_return_address(0),
get_dev_name(dev), qsize, *in, *out);
udelay(100);
} while (cnt--);
if (dev == IPC_FMT) {
dpram_trigger_force_cp_crash(dpld);
} else {
set_tx_head(dpld, dev, 0);
set_tx_tail(dpld, dev, 0);
}
*in = 0;
*out = 0;
return -EINVAL;
}
/*
* func: restruct a new udp package.
* param:
* return: 0 success; -1 fail
*/
int restruct_pkt(char *buf,
struct eth_hdr *pethh,
struct ip_hdr *piph, int iph_len,
struct udp_hdr *pudph,
uint32_t multi_ip,
uint16_t multi_port)
{
char errbuf[PCAP_ERRBUF_SIZE] = {0};
// char * device = "eth0";
char device [DEV_BUF_SIZE];
pcap_t *adhandle = NULL;
int ret;
char *str_dev = get_dev_name(piph->ip_destaddr, device);
if(str_dev == NULL)
{
fprintf(gfp_log, "[%s:%d]get_dev_name is null!\n", __FILE__, __LINE__);
fflush(gfp_log);
return -1;
}
if((adhandle = pcap_open_live(device, 0x10000, PCAP_OPENFLAG_PROMISCUOUS, 1000, errbuf)) == NULL)
{
fprintf(gfp_log, "[%s:%d][pcap_open_live error] : %s\n", __FILE__, __LINE__, errbuf);
fflush(gfp_log);
return -1;
}
pethh->ether_dhost[0] = 0x01;
pethh->ether_dhost[1] = 0x00;
pethh->ether_dhost[2] = 0x5e;
pethh->ether_dhost[3] = (unsigned char)((multi_ip >> 8) & 0x7F);;
pethh->ether_dhost[4] = (unsigned char)((multi_ip >> 16) & 0xFF);;
pethh->ether_dhost[5] = (unsigned char)((multi_ip >> 24) & 0xFF);;
//AC:85:3D:AF:C7:08
pethh->ether_shost[0] = 0x00;
pethh->ether_shost[1] = 0x16;
pethh->ether_shost[2] = 0x3e;
pethh->ether_shost[3] = 0x00;
pethh->ether_shost[4] = 0x04;
pethh->ether_shost[5] = 0x0e;
pethh->ether_type = htons(ETHERTYPE_IP);
if((sizeof(struct ip_hdr) % 4) != 0)
{
fprintf(gfp_log, "[%s:%d][IP Header error]\n", __FILE__, __LINE__);
fflush(gfp_log);
pcap_close(adhandle);
return -1;
}
uint16_t ip_len = ntohs(piph->ip_totallength);
uint16_t udp_len = htons(pudph->udp_length);
piph->ip_tos = 0;
piph->ip_id = htons(0x0000);
piph->ip_offset = htons(0x4000);
piph->ip_ttl = 0x20;
piph->ip_checksum = 0;
piph->ip_destaddr = multi_ip;
piph->ip_totallength = htons(ip_len);
piph->ip_checksum = checksum((uint16_t *)piph, iph_len);
pudph->dest_portno = htons(multi_port);
pudph->src_portno = htons(52540);
pudph->udp_checksum = 0;
pudph->udp_length = htons(udp_len);
pudph->udp_checksum = udp_checksum(piph, iph_len, udp_len);
//pudph->udp_checksum = htons(0x16ed);
//////just for test printf
//printf("rebuild multicast package: \n");
//printf(" src : %s" , inet_ntoa(*((struct in_addr *)(&piph->ip_srcaddr))));
//printf(" dst : %s\n" , inet_ntoa(*((struct in_addr *)(&piph->ip_destaddr))));
///////////////////////////////////////////////////
int pkt_len = ip_len + ETHER_HEADER_SIZE;
/* just print the pkt info */
/*
int i=0;
for(i = 0 ; i != sizeof(struct ip_hdr); i++)
{
printf("%02x ",(*((char *)piph+i))&0xff);
}
*/
//printf("pkt len %u\n", pkt_len);
ret = write(tun, piph, ip_len);
if(pcap_sendpacket(adhandle, (const u_char*)buf, pkt_len) == -1)
{
fprintf(gfp_log, "[%s:%d][pcap_sendpacket error]\n", __FILE__, __LINE__);
fflush(gfp_log);
pcap_close(adhandle);
return -1;
}
else
{
// printf("=====send a pkt!\n");
}
pcap_close(adhandle);
return 0;
}
struct link_device *dpram_create_link_device(struct platform_device *pdev)
{
struct modem_data *mdm_data = NULL;
struct dpram_link_device *dpld = NULL;
struct link_device *ld = NULL;
struct resource *res = NULL;
resource_size_t res_size;
struct modemlink_dpram_control *dpctl = NULL;
unsigned long task_data;
int ret = 0;
int i = 0;
int bsize;
int qsize;
/* Get the platform data */
mdm_data = (struct modem_data *)pdev->dev.platform_data;
if (!mdm_data) {
mif_info("ERR! mdm_data == NULL\n");
goto err;
}
mif_info("modem = %s\n", mdm_data->name);
mif_info("link device = %s\n", mdm_data->link_name);
if (!mdm_data->dpram_ctl) {
mif_info("ERR! mdm_data->dpram_ctl == NULL\n");
goto err;
}
dpctl = mdm_data->dpram_ctl;
/* Alloc DPRAM link device structure */
dpld = kzalloc(sizeof(struct dpram_link_device), GFP_KERNEL);
if (!dpld) {
mif_info("ERR! kzalloc dpld fail\n");
goto err;
}
ld = &dpld->ld;
task_data = (unsigned long)dpld;
/* Retrieve modem data and DPRAM control data from the modem data */
ld->mdm_data = mdm_data;
ld->name = mdm_data->link_name;
ld->ipc_version = mdm_data->ipc_version;
/* Retrieve the most basic data for IPC from the modem data */
dpld->dpctl = dpctl;
dpld->dp_type = dpctl->dp_type;
if (mdm_data->ipc_version < SIPC_VER_50) {
if (!dpctl->max_ipc_dev) {
mif_info("ERR! no max_ipc_dev\n");
goto err;
}
ld->aligned = dpctl->aligned;
dpld->max_ipc_dev = dpctl->max_ipc_dev;
} else {
ld->aligned = 1;
dpld->max_ipc_dev = MAX_SIPC5_DEV;
}
/* Set attributes as a link device */
ld->init_comm = dpram_link_init;
ld->terminate_comm = dpram_link_terminate;
ld->send = dpram_send;
ld->force_dump = dpram_force_dump;
ld->dump_start = dpram_dump_start;
ld->dump_update = dpram_dump_update;
ld->ioctl = dpram_ioctl;
INIT_LIST_HEAD(&ld->list);
skb_queue_head_init(&ld->sk_fmt_tx_q);
skb_queue_head_init(&ld->sk_raw_tx_q);
skb_queue_head_init(&ld->sk_rfs_tx_q);
ld->skb_txq[IPC_FMT] = &ld->sk_fmt_tx_q;
ld->skb_txq[IPC_RAW] = &ld->sk_raw_tx_q;
ld->skb_txq[IPC_RFS] = &ld->sk_rfs_tx_q;
/* Set up function pointers */
dpram_setup_common_op(dpld);
dpld->dpram_dump = dpram_dump_memory;
dpld->ext_op = dpram_get_ext_op(mdm_data->modem_type);
/* Retrieve DPRAM resource */
if (!dpctl->dp_base) {
res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
if (!res) {
mif_info("%s: ERR! platform_get_resource fail\n",
ld->name);
goto err;
}
res_size = resource_size(res);
dpctl->dp_base = ioremap_nocache(res->start, res_size);
dpctl->dp_size = res_size;
}
dpld->dp_base = dpctl->dp_base;
dpld->dp_size = dpctl->dp_size;
mif_info("%s: dp_type %d, aligned %d, dp_base 0x%08X, dp_size %d\n",
ld->name, dpld->dp_type, ld->aligned, (int)dpld->dp_base,
dpld->dp_size);
/* Initialize DPRAM map (physical map -> logical map) */
ret = dpram_table_init(dpld);
if (ret < 0) {
mif_info("%s: ERR! dpram_table_init fail (err %d)\n",
ld->name, ret);
goto err;
}
/* Disable IPC */
set_magic(dpld, 0);
set_access(dpld, 0);
dpld->dpram_init_status = DPRAM_INIT_STATE_NONE;
/* Initialize locks, completions, and bottom halves */
snprintf(dpld->wlock_name, DP_MAX_NAME_LEN, "%s_wlock", ld->name);
wake_lock_init(&dpld->wlock, WAKE_LOCK_SUSPEND, dpld->wlock_name);
init_completion(&dpld->dpram_init_cmd);
init_completion(&dpld->modem_pif_init_done);
init_completion(&dpld->udl_start_complete);
init_completion(&dpld->udl_cmd_complete);
init_completion(&dpld->crash_start_complete);
init_completion(&dpld->dump_start_complete);
init_completion(&dpld->dump_recv_done);
tasklet_init(&dpld->rx_tsk, dpram_ipc_rx_task, task_data);
if (dpld->ext_op && dpld->ext_op->dl_task)
tasklet_init(&dpld->dl_tsk, dpld->ext_op->dl_task, task_data);
/* Prepare RXB queue */
qsize = DPRAM_MAX_RXBQ_SIZE;
for (i = 0; i < dpld->max_ipc_dev; i++) {
bsize = rxbq_get_page_size(get_rx_buff_size(dpld, i));
dpld->rxbq[i].size = qsize;
dpld->rxbq[i].in = 0;
dpld->rxbq[i].out = 0;
dpld->rxbq[i].rxb = rxbq_create_pool(bsize, qsize);
if (!dpld->rxbq[i].rxb) {
mif_info("%s: ERR! %s rxbq_create_pool fail\n",
ld->name, get_dev_name(i));
goto err;
}
mif_info("%s: %s rxbq_pool created (bsize:%d, qsize:%d)\n",
ld->name, get_dev_name(i), bsize, qsize);
}
/* Prepare a multi-purpose miscellaneous buffer */
dpld->buff = kzalloc(dpld->dp_size, GFP_KERNEL);
if (!dpld->buff) {
mif_info("%s: ERR! kzalloc dpld->buff fail\n", ld->name);
goto err;
}
/* Retrieve DPRAM IRQ GPIO# */
dpld->gpio_dpram_int = mdm_data->gpio_dpram_int;
/* Retrieve DPRAM IRQ# */
if (!dpctl->dpram_irq) {
dpctl->dpram_irq = platform_get_irq_byname(pdev, "dpram_irq");
if (dpctl->dpram_irq < 0) {
mif_info("%s: ERR! platform_get_irq_byname fail\n",
ld->name);
goto err;
}
}
dpld->irq = dpctl->dpram_irq;
/* Retrieve DPRAM IRQ flags */
if (!dpctl->dpram_irq_flags)
dpctl->dpram_irq_flags = (IRQF_NO_SUSPEND | IRQF_TRIGGER_LOW);
dpld->irq_flags = dpctl->dpram_irq_flags;
/* Register DPRAM interrupt handler */
snprintf(dpld->irq_name, DP_MAX_NAME_LEN, "%s_irq", ld->name);
ret = dpram_register_isr(dpld->irq, dpram_irq_handler, dpld->irq_flags,
dpld->irq_name, dpld);
if (ret)
goto err;
return ld;
err:
if (dpld) {
if (dpld->buff)
kfree(dpld->buff);
kfree(dpld);
}
return NULL;
}
int main(int argc, char *argv[])
{
pj_caching_pool cp;
pjmedia_endpt *med_endpt;
int id = -1, verbose = 0;
int clock_rate = 8000;
int frame = -1;
int channel = 1;
struct pj_getopt_option long_options[] = {
{ "id", 1, 0, 'i' },
{ "rate", 1, 0, 'r' },
{ "frame", 1, 0, 'f' },
{ "channel", 1, 0, 'n' },
{ "verbose", 0, 0, 'v' },
{ "help", 0, 0, 'h' },
{ NULL, 0, 0, 0 }
};
int c, option_index;
pj_status_t status;
/* Init pjlib */
status = pj_init();
PJ_ASSERT_RETURN(status==PJ_SUCCESS, 1);
/* Must create a pool factory before we can allocate any memory. */
pj_caching_pool_init(&cp, &pj_pool_factory_default_policy, 0);
/*
* Initialize media endpoint.
* This will implicitly initialize PJMEDIA too.
*/
status = pjmedia_endpt_create(&cp.factory, NULL, 1, &med_endpt);
PJ_ASSERT_RETURN(status == PJ_SUCCESS, 1);
/* Print devices */
enum_devices();
/* Parse options */
pj_optind = 0;
while((c=pj_getopt_long(argc,argv, "i:r:f:n:vh",
long_options, &option_index))!=-1)
{
switch (c) {
case 'i':
id = atoi(pj_optarg);
break;
case 'r':
clock_rate = atoi(pj_optarg);
break;
case 'f':
frame = atoi(pj_optarg);
break;
case 'n':
channel = atoi(pj_optarg);
break;
case 'v':
verbose = 1;
break;
case 'h':
puts(desc);
return 0;
break;
default:
printf("Error: invalid options %s\n", argv[pj_optind-1]);
puts(desc);
return 1;
}
}
if (pj_optind != argc) {
printf("Error: invalid options\n");
puts(desc);
return 1;
}
if (!verbose)
pj_log_set_level(3);
if (frame == -1)
frame = 10 * clock_rate / 1000;
status = perform_test(get_dev_name(id), id, PJMEDIA_DIR_CAPTURE_PLAYBACK,
clock_rate, frame, channel, verbose);
if (status != 0)
return 1;
return 0;
}
/*
ret < 0 : error
ret == 0 : no data
ret > 0 : valid data
*/
static int dpram_ipc_recv_data_with_skb(struct dpram_link_device *dpld, int dev)
{
struct link_device *ld = &dpld->ld;
struct io_device *iod = dpld->iod[dev];
struct sk_buff *skb;
u8 __iomem *src = get_rx_buff(dpld, dev);
u32 qsize = get_rx_buff_size(dpld, dev);
u32 in;
u32 out;
u32 rcvd;
int rest;
u8 *frm;
u8 *dst;
unsigned int len;
unsigned int pad;
unsigned int tot;
struct mif_irq_map map;
rcvd = dpram_get_rxq_rcvd(dpld, dev, qsize, &in, &out);
if (rcvd <= 0)
return rcvd;
if (dev == IPC_FMT) {
set_dpram_map(dpld, &map);
mif_irq_log(ld->mc->msd, map, "ipc_recv", sizeof("ipc_recv"));
}
rest = rcvd;
while (rest > 0) {
frm = src + out;
if (unlikely(!sipc5_start_valid(frm[0]))) {
mif_err("%s: ERR! %s invalid start 0x%02X\n",
ld->name, get_dev_name(dev), frm[0]);
skb_queue_purge(&dpld->skb_rxq[dev]);
return -EBADMSG;
}
len = sipc5_get_frame_sz16(frm);
if (unlikely(len > rest)) {
mif_err("%s: ERR! %s len %d > rest %d\n",
ld->name, get_dev_name(dev), len, rest);
skb_queue_purge(&dpld->skb_rxq[dev]);
return -EBADMSG;
}
pad = sipc5_calc_padding_size(len);
tot = len + pad;
/* Allocate an skb */
skb = dev_alloc_skb(tot);
if (!skb) {
mif_err("%s: ERR! %s dev_alloc_skb fail\n",
ld->name, get_dev_name(dev));
return -ENOMEM;
}
/* Read data from each DPRAM buffer */
dst = skb_put(skb, tot);
dpram_ipc_read(dpld, dev, dst, src, out, tot, qsize);
skb_trim(skb, len);
iod->recv_skb(iod, ld, skb);
/* Calculate and set new out */
rest -= tot;
out += tot;
if (out >= qsize)
out -= qsize;
}
set_rx_tail(dpld, dev, out);
return rcvd;
}
