void init_package_head(ap_package_t *pkt, U8 data_type,U8 src_addr, U8 dst_addr, U8 port, U8 ttl, U8 type)
{
memset(pkt, 0, sizeof(ap_package_t));
set_src_addr(pkt, src_addr);
set_dst_addr(pkt, dst_addr);
set_port(pkt, port);
set_ttl(pkt, ttl);//赋值为7
set_type(pkt, type);//type传入参数为0
set_CoS(pkt,data_type);
}
static int s3c_pp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
s3c_pp_instance_context_t *current_instance;
s3c_pp_params_t *parg;
unsigned int temp = 0;
mutex_lock(h_mutex);
current_instance = (s3c_pp_instance_context_t *) file->private_data;
parg = (s3c_pp_params_t *) arg;
switch ( cmd )
{
case S3C_PP_SET_PARAMS:
{
s3c_pp_out_path_t temp_out_path;
unsigned int temp_src_width, temp_src_height, temp_dst_width, temp_dst_height;
s3c_color_space_t temp_src_color_space, temp_dst_color_space;
get_user(temp_out_path, &parg->out_path);
if ( (-1 != s3c_pp_instance_info.fifo_mode_instance_no )
|| ((s3c_pp_instance_info.dma_mode_instance_count) && (FIFO_FREERUN == temp_out_path)) )
{
printk ( KERN_ERR "\n%s: S3C_PP_SET_PARAMS can't be executed.\n", __FUNCTION__ );
mutex_unlock(h_mutex);
return -EINVAL;
}
get_user(temp_src_width, &parg->src_width);
get_user(temp_src_height, &parg->src_height);
get_user(temp_dst_width, &parg->dst_width);
get_user(temp_dst_height, &parg->dst_height);
// S3C6410 support that the source image is up to 4096 x 4096
// and the destination image is up to 2048 x 2048.
if ( (temp_src_width > 4096) || (temp_src_height > 4096)
|| (temp_dst_width > 2048) || (temp_dst_height > 2048) )
{
printk(KERN_ERR "\n%s: Size is too big to be supported.\n", __FUNCTION__);
mutex_unlock(h_mutex);
return -EINVAL;
}
get_user(temp_src_color_space, &parg->src_color_space);
get_user(temp_dst_color_space, &parg->dst_color_space);
if ( ( (temp_src_color_space == YC420) && (temp_src_width % 8) )
|| ( (temp_src_color_space == RGB16) && (temp_src_width % 2) )
|| ( (temp_out_path == DMA_ONESHOT) && ( ((temp_dst_color_space == YC420) && (temp_dst_width % 8))
|| ((temp_dst_color_space == RGB16) && (temp_dst_width % 2)))) )
{
printk(KERN_ERR "\n%s: YUV420 image width must be a multiple of 8.\n", __FUNCTION__);
printk(KERN_ERR "%s: RGB16 must be a multiple of 2.\n", __FUNCTION__);
mutex_unlock(h_mutex);
return -EINVAL;
}
get_user(current_instance->src_full_width, &parg->src_full_width);
get_user(current_instance->src_full_height, &parg->src_full_height);
get_user(current_instance->src_start_x, &parg->src_start_x);
get_user(current_instance->src_start_y, &parg->src_start_y);
current_instance->src_width = temp_src_width;
current_instance->src_height = temp_src_height;
current_instance->src_color_space = temp_src_color_space;
get_user(current_instance->dst_full_width, &parg->dst_full_width);
get_user(current_instance->dst_full_height, &parg->dst_full_height);
get_user(current_instance->dst_start_x, &parg->dst_start_x);
get_user(current_instance->dst_start_y, &parg->dst_start_y);
current_instance->dst_width = temp_dst_width;
current_instance->dst_height = temp_dst_height;
current_instance->dst_color_space = temp_dst_color_space;
current_instance->out_path = temp_out_path;
if ( DMA_ONESHOT == current_instance->out_path )
{
s3c_pp_instance_info.instance_state[current_instance->instance_no] = PP_INSTANCE_INUSE_DMA_ONESHOT;
s3c_pp_instance_info.dma_mode_instance_count++;
}
else
{
get_user(current_instance->scan_mode, &parg->scan_mode);
current_instance->dst_color_space = RGB30;
s3c_pp_instance_info.instance_state[current_instance->instance_no] = PP_INSTANCE_INUSE_FIFO_FREERUN;
s3c_pp_instance_info.fifo_mode_instance_no = current_instance->instance_no;
s3c_pp_instance_info.wincon0_value_before_fifo_mode = __raw_readl ( S3C_WINCON0 );
//.[ REDUCE_VCLK_SYOP_TIME
if ( current_instance->src_height > current_instance->dst_height )
{
int i;
for ( i=2; (current_instance->src_height >= (i * current_instance->dst_height)) && (i<8); i++ )
{
}
current_instance->src_full_width *= i;
current_instance->src_full_height /= i;
current_instance->src_height /= i;
}
//.] REDUCE_VCLK_SYOP_TIME
}
current_instance->value_changed |= PP_VALUE_CHANGED_PARAMS;
}
break;
case S3C_PP_START:
dprintk ( "%s: S3C_PP_START last_instance=%d, curr_instance=%d\n", __FUNCTION__,
s3c_pp_instance_info.last_running_instance_no, current_instance->instance_no );
if ( PP_INSTANCE_READY == s3c_pp_instance_info.instance_state[current_instance->instance_no] )
{
printk ( KERN_ERR "%s: S3C_PP_START must be executed after running S3C_PP_SET_PARAMS.\n", __FUNCTION__ );
mutex_unlock(h_mutex);
return -EINVAL;
}
if ( current_instance->instance_no != s3c_pp_instance_info.last_running_instance_no )
{
__raw_writel(0x0<<31, s3c_pp_base + S3C_VPP_POSTENVID);
temp = S3C_MODE2_ADDR_CHANGE_DISABLE | S3C_MODE2_CHANGE_AT_FRAME_END | S3C_MODE2_SOFTWARE_TRIGGER;
__raw_writel(temp, s3c_pp_base + S3C_VPP_MODE_2);
set_clock_src(HCLK);
// setting the src/dst color space
set_data_format(current_instance);
// setting the src/dst size
set_scaler(current_instance);
// setting the src/dst buffer address
set_src_addr(current_instance);
set_dest_addr(current_instance);
current_instance->value_changed = PP_VALUE_CHANGED_NONE;
s3c_pp_instance_info.last_running_instance_no = current_instance->instance_no;
s3c_pp_instance_info.running_instance_no = current_instance->instance_no;
if ( PP_INSTANCE_INUSE_DMA_ONESHOT == s3c_pp_instance_info.instance_state[current_instance->instance_no] )
{ // DMA OneShot Mode
dprintk ( "%s: DMA_ONESHOT mode\n", __FUNCTION__ );
post_int_enable(1);
pp_dma_mode_set_and_start();
if ( !(file->f_flags & O_NONBLOCK) )
{
if (interruptible_sleep_on_timeout(&waitq, 500) == 0)
{
printk(KERN_ERR "\n%s: Waiting for interrupt is timeout\n", __FUNCTION__);
}
}
}
else
{ // FIFO freerun Mode
dprintk ( "%s: FIFO_freerun mode\n", __FUNCTION__ );
s3c_pp_instance_info.fifo_mode_instance_no = current_instance->instance_no;
post_int_enable(1);
pp_fifo_mode_set_and_start(current_instance);
}
}
else
{
if ( current_instance->value_changed != PP_VALUE_CHANGED_NONE )
{
__raw_writel(0x0<<31, s3c_pp_base + S3C_VPP_POSTENVID);
if ( current_instance->value_changed & PP_VALUE_CHANGED_PARAMS )
{
set_data_format(current_instance);
set_scaler(current_instance);
}
if ( current_instance->value_changed & PP_VALUE_CHANGED_SRC_BUF_ADDR_PHY )
{
set_src_addr(current_instance);
}
if ( current_instance->value_changed & PP_VALUE_CHANGED_DST_BUF_ADDR_PHY )
{
set_dest_addr(current_instance);
}
current_instance->value_changed = PP_VALUE_CHANGED_NONE;
}
s3c_pp_instance_info.running_instance_no = current_instance->instance_no;
post_int_enable(1);
start_processing();
if ( !(file->f_flags & O_NONBLOCK) )
{
if (interruptible_sleep_on_timeout(&waitq, 500) == 0)
{
printk(KERN_ERR "\n%s: Waiting for interrupt is timeout\n", __FUNCTION__);
}
}
}
break;
case S3C_PP_GET_SRC_BUF_SIZE:
if ( PP_INSTANCE_READY == s3c_pp_instance_info.instance_state[current_instance->instance_no] )
{
dprintk ( "%s: S3C_PP_GET_SRC_BUF_SIZE must be executed after running S3C_PP_SET_PARAMS.\n", __FUNCTION__ );
mutex_unlock(h_mutex);
return -EINVAL;
}
temp = cal_data_size ( current_instance->src_color_space, current_instance->src_full_width, current_instance->src_full_height );
mutex_unlock(h_mutex);
return temp;
case S3C_PP_SET_SRC_BUF_ADDR_PHY:
get_user(current_instance->src_buf_addr_phy, &parg->src_buf_addr_phy);
current_instance->value_changed |= PP_VALUE_CHANGED_SRC_BUF_ADDR_PHY;
break;
case S3C_PP_SET_SRC_BUF_NEXT_ADDR_PHY:
if ( current_instance->instance_no != s3c_pp_instance_info.fifo_mode_instance_no )
{ // if FIFO Mode is not Active
dprintk (KERN_DEBUG "%s: S3C_PP_SET_SRC_BUF_NEXT_ADDR_PHY can't be executed.\n", __FUNCTION__ );
mutex_unlock(h_mutex);
return -EINVAL;
}
get_user(current_instance->src_next_buf_addr_phy, &parg->src_next_buf_addr_phy);
temp = __raw_readl(s3c_pp_base + S3C_VPP_MODE_2);
temp |= (0x1<<4);
__raw_writel(temp, s3c_pp_base + S3C_VPP_MODE_2);
set_src_next_buf_addr(current_instance);
temp = __raw_readl(s3c_pp_base + S3C_VPP_MODE_2);
temp &= ~(0x1<<4);
__raw_writel(temp, s3c_pp_base + S3C_VPP_MODE_2);
break;
case S3C_PP_GET_DST_BUF_SIZE:
if ( PP_INSTANCE_READY == s3c_pp_instance_info.instance_state[current_instance->instance_no] )
{
dprintk ( "%s: S3C_PP_GET_DST_BUF_SIZE must be executed after running S3C_PP_SET_PARAMS.\n", __FUNCTION__ );
mutex_unlock(h_mutex);
return -EINVAL;
}
temp = cal_data_size ( current_instance->dst_color_space, current_instance->dst_full_width, current_instance->dst_full_height );
mutex_unlock(h_mutex);
return temp;
case S3C_PP_SET_DST_BUF_ADDR_PHY:
get_user(current_instance->dst_buf_addr_phy, &parg->dst_buf_addr_phy);
current_instance->value_changed |= PP_VALUE_CHANGED_DST_BUF_ADDR_PHY;
break;
case S3C_PP_ALLOC_KMEM:
{
s3c_pp_mem_alloc_t param;
if (copy_from_user(¶m, (s3c_pp_mem_alloc_t *)arg, sizeof(s3c_pp_mem_alloc_t)))
{
mutex_unlock(h_mutex);
return -EFAULT;
}
flag = ALLOC_KMEM;
param.vir_addr = do_mmap(file, 0, param.size, PROT_READ|PROT_WRITE, MAP_SHARED, 0);
dprintk (KERN_DEBUG "param.vir_addr = %08x\n", param.vir_addr);
flag = 0;
if(param.vir_addr == -EINVAL) {
printk(KERN_ERR "%s: PP_MEM_ALLOC FAILED\n", __FUNCTION__);
mutex_unlock(h_mutex);
return -EFAULT;
}
param.phy_addr = physical_address;
dprintk (KERN_DEBUG "KERNEL MALLOC : param.phy_addr = 0x%X \t size = %d \t param.vir_addr = 0x%X\n", param.phy_addr, param.size, param.vir_addr);
if (copy_to_user((s3c_pp_mem_alloc_t *)arg, ¶m, sizeof(s3c_pp_mem_alloc_t)))
{
mutex_unlock(h_mutex);
return -EFAULT;
}
}
break;
case S3C_PP_FREE_KMEM:
{
s3c_pp_mem_alloc_t param;
struct mm_struct *mm = current->mm;
void *virt_addr;
if ( copy_from_user(¶m, (s3c_pp_mem_alloc_t *)arg, sizeof(s3c_pp_mem_alloc_t)) )
{
mutex_unlock(h_mutex);
return -EFAULT;
}
dprintk (KERN_DEBUG "KERNEL FREE : param.phy_addr = 0x%X \t size = %d \t param.vir_addr = 0x%X\n", param.phy_addr, param.size, param.vir_addr);
if ( do_munmap(mm, param.vir_addr, param.size ) < 0 )
{
dprintk("do_munmap() failed !!\n");
mutex_unlock(h_mutex);
return -EINVAL;
}
virt_addr = phys_to_virt(param.phy_addr);
dprintk ( "KERNEL : virt_addr = 0x%X\n", (unsigned int) virt_addr );
kfree(virt_addr);
param.size = 0;
dprintk(KERN_DEBUG "do_munmap() succeed !!\n");
}
break;
case S3C_PP_GET_RESERVED_MEM_SIZE:
mutex_unlock(h_mutex);
return PP_RESERVED_MEM_SIZE;
case S3C_PP_GET_RESERVED_MEM_ADDR_PHY:
mutex_unlock(h_mutex);
return PP_RESERVED_MEM_ADDR_PHY;
default:
mutex_unlock(h_mutex);
return -EINVAL;
}
mutex_unlock(h_mutex);
return 0;
}
int main(int argc, char** argv)
{
std::cout << "\nsend_rpc_ra\n";
std::cout << "\n(C) 2015 Alexander Holler\n\n";
if (argc != 4) {
std::cout << "Usage: " <<
"send_rpc_ra interface destination_ipv6 prefix_ipv6\n" <<
"Example: " <<
"send_ra eth0 ff02::1 fecd::\n\n";
return 1;
}
std::string interface(argv[1]);
std::string destination(argv[2]);
std::string prefix(argv[3]);
struct {
nd_router_advert nra;
nd_opt_prefix_info opt_prefix_info;
} my_ra;
std::memset(&my_ra, 0, sizeof(my_ra));
my_ra.nra.nd_ra_type = ND_ROUTER_ADVERT;
msghdr msghdr;
std::memset(&msghdr, 0, sizeof(msghdr));
// destination address
sockaddr_in6 dst;
std::memset(&dst, 0, sizeof(dst));
dst.sin6_family = AF_INET6;
dst.sin6_port = htons(IPPROTO_ICMPV6);
if (inet_pton(AF_INET6, destination.c_str(), &dst.sin6_addr) != 1) {
std::cerr << "Error setting destination '" << destination << "'\n";
return 2;
}
msghdr.msg_name = &dst;
msghdr.msg_namelen = sizeof(dst);
iovec iov[2];
std::memset(&iov, 0, sizeof(iov));
iov[0].iov_base = &my_ra;
iov[0].iov_len = sizeof(my_ra);
msghdr.msg_iov = (struct iovec *) &iov;
msghdr.msg_iovlen = sizeof(iov) / sizeof(struct iovec);
in6_pktinfo* ipi;
cmsghdr* cmsg_hdr;
uint8_t cmsgbuf[CMSG_SPACE(sizeof(int)) + CMSG_SPACE(sizeof(*ipi))];
std::memset(&cmsgbuf, 0, sizeof(cmsgbuf));
msghdr.msg_control = &cmsgbuf;
msghdr.msg_controllen = sizeof(cmsgbuf);
// hop limit
cmsg_hdr = CMSG_FIRSTHDR(&msghdr);
cmsg_hdr->cmsg_level = IPPROTO_IPV6;
cmsg_hdr->cmsg_type = IPV6_HOPLIMIT;
cmsg_hdr->cmsg_len = CMSG_LEN(sizeof(int));
cmsgbuf[sizeof(*cmsg_hdr)] = 255; // using CMSG_DATA throws a warning
// packet info
cmsg_hdr = CMSG_NXTHDR(&msghdr, cmsg_hdr);
cmsg_hdr->cmsg_level = IPPROTO_IPV6;
cmsg_hdr->cmsg_type = IPV6_PKTINFO;
cmsg_hdr->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
ipi = (struct in6_pktinfo *) CMSG_DATA(cmsg_hdr);
ipi->ipi6_ifindex = if_nametoindex(interface.c_str());
if (!ipi->ipi6_ifindex) {
std::cerr << "Interface '" << interface << "' not found!\n";
return 3;
}
in6_addr s_addr;
std::memset(&s_addr, 0, sizeof(s_addr));
if (set_src_addr(interface, s_addr)) {
std::cerr << "Error finding link-local address of interface '" << interface << "'!\n";
return 4;
}
std::memcpy(&ipi->ipi6_addr, &s_addr, sizeof(ipi->ipi6_addr));
msghdr.msg_iovlen = 1;
my_ra.opt_prefix_info.nd_opt_pi_type = ND_OPT_PREFIX_INFORMATION;
my_ra.opt_prefix_info.nd_opt_pi_len = 4;
if (inet_pton(AF_INET6, prefix.c_str(), &my_ra.opt_prefix_info.nd_opt_pi_prefix) != 1) {
std::cerr << "Error converting prefix '" << prefix << "'!\n";
return 5;
}
my_ra.opt_prefix_info.nd_opt_pi_prefix_len = 64;
if (check_prefix(interface, my_ra.opt_prefix_info.nd_opt_pi_prefix)) {
std::cerr << "Prefix " << prefix << " seems to be in use!\n";
return 6;
}
my_ra.opt_prefix_info.nd_opt_pi_flags_reserved |= ND_OPT_PI_FLAG_AUTO;
my_ra.opt_prefix_info.nd_opt_pi_flags_reserved |= ND_OPT_PI_FLAG_RADDR;
// Setting both lifetimes to 1 means the kernel will only delete the
// link-local address without creating it before.
my_ra.opt_prefix_info.nd_opt_pi_valid_time = htonl(1);
my_ra.opt_prefix_info.nd_opt_pi_preferred_time = htonl(1);
int sock = ::socket(AF_INET6, SOCK_RAW | SOCK_CLOEXEC, IPPROTO_ICMPV6);
if (sock < 0) {
std::cerr << "Error opening raw socket, are you root?\n";
return 7;
}
if (::sendmsg(sock, &msghdr, 0) < 0) {
::close(sock);
std::cerr << "Error sending RA ( " << strerror(errno) << ")!\n";
return 8;
}
::close(sock);
std::cout << "Sent a Router Advertisment with prefix " <<
prefix << " to " << destination << "\n";
return 0;
}
