1 /* 2 * openwifi side channel driver 3 * Author: Xianjun Jiao 4 * SPDX-FileCopyrightText: 2019 UGent 5 * SPDX-License-Identifier: AGPL-3.0-or-later 6 */ 7 8 #include <linux/bitops.h> 9 #include <linux/dmapool.h> 10 #include <linux/dma/xilinx_dma.h> 11 #include <linux/init.h> 12 #include <linux/interrupt.h> 13 #include <linux/io.h> 14 #include <linux/iopoll.h> 15 #include <linux/module.h> 16 #include <linux/of_address.h> 17 #include <linux/of_dma.h> 18 #include <linux/of_platform.h> 19 #include <linux/of_irq.h> 20 #include <linux/slab.h> 21 #include <linux/clk.h> 22 #include <linux/io-64-nonatomic-lo-hi.h> 23 #include <linux/delay.h> 24 #include <linux/dmaengine.h> 25 26 #include <net/sock.h> 27 #include <linux/netlink.h> 28 #include <linux/skbuff.h> 29 30 #include "side_ch.h" 31 32 static int num_eq_init = 8; // should be 0~8 33 static int iq_len_init = 0; //if iq_len>0, iq capture enabled, csi disabled 34 35 module_param(num_eq_init, int, 0); 36 MODULE_PARM_DESC(num_eq_init, "num_eq_init. 0~8. number of equalizer output (52 each) appended to CSI"); 37 38 module_param(iq_len_init, int, 0); 39 MODULE_PARM_DESC(iq_len_init, "iq_len_init. if iq_len_init>0, iq capture enabled, csi disabled"); 40 41 static void __iomem *base_addr; // to store driver specific base address needed for mmu to translate virtual address to physical address in our FPGA design 42 43 struct dma_chan *chan_to_pl = NULL; 44 struct dma_chan *chan_to_ps = NULL; 45 u8 *side_info_buf = NULL; 46 dma_cookie_t chan_to_ps_cookie; 47 const int max_side_info_buf_size = MAX_NUM_DMA_SYMBOL*8; 48 49 /* IO accessors */ 50 static inline u32 reg_read(u32 reg) 51 { 52 return ioread32(base_addr + reg); 53 } 54 55 static inline void reg_write(u32 reg, u32 value) 56 { 57 iowrite32(value, base_addr + reg); 58 } 59 60 static inline void SIDE_CH_REG_MULTI_RST_write(u32 Data) { 61 reg_write(SIDE_CH_REG_MULTI_RST_ADDR, Data); 62 } 63 64 static inline u32 SIDE_CH_REG_CONFIG_read(void){ 65 return reg_read(SIDE_CH_REG_CONFIG_ADDR); 66 } 67 68 static inline void SIDE_CH_REG_CONFIG_write(u32 value){ 69 reg_write(SIDE_CH_REG_CONFIG_ADDR, value); 70 } 71 72 static inline u32 SIDE_CH_REG_NUM_DMA_SYMBOL_read(void){ 73 return reg_read(SIDE_CH_REG_NUM_DMA_SYMBOL_ADDR); 74 } 75 76 static inline void SIDE_CH_REG_NUM_DMA_SYMBOL_write(u32 value){ 77 reg_write(SIDE_CH_REG_NUM_DMA_SYMBOL_ADDR, value); 78 } 79 80 static inline u32 SIDE_CH_REG_IQ_CAPTURE_read(void){ 81 return reg_read(SIDE_CH_REG_IQ_CAPTURE_ADDR); 82 } 83 84 static inline void SIDE_CH_REG_IQ_CAPTURE_write(u32 value){ 85 reg_write(SIDE_CH_REG_IQ_CAPTURE_ADDR, value); 86 } 87 88 static inline u32 SIDE_CH_REG_NUM_EQ_read(void){ 89 return reg_read(SIDE_CH_REG_NUM_EQ_ADDR); 90 } 91 92 static inline void SIDE_CH_REG_NUM_EQ_write(u32 value){ 93 reg_write(SIDE_CH_REG_NUM_EQ_ADDR, value); 94 } 95 96 static inline u32 SIDE_CH_REG_FC_TARGET_read(void){ 97 return reg_read(SIDE_CH_REG_FC_TARGET_ADDR); 98 } 99 100 static inline void SIDE_CH_REG_FC_TARGET_write(u32 value){ 101 reg_write(SIDE_CH_REG_FC_TARGET_ADDR, value); 102 } 103 104 static inline u32 SIDE_CH_REG_ADDR1_TARGET_read(void){ 105 return reg_read(SIDE_CH_REG_ADDR1_TARGET_ADDR); 106 } 107 108 static inline void SIDE_CH_REG_ADDR1_TARGET_write(u32 value){ 109 reg_write(SIDE_CH_REG_ADDR1_TARGET_ADDR, value); 110 } 111 112 static inline u32 SIDE_CH_REG_ADDR2_TARGET_read(void){ 113 return reg_read(SIDE_CH_REG_ADDR2_TARGET_ADDR); 114 } 115 116 static inline void SIDE_CH_REG_ADDR2_TARGET_write(u32 value){ 117 reg_write(SIDE_CH_REG_ADDR2_TARGET_ADDR, value); 118 } 119 120 static inline u32 SIDE_CH_REG_IQ_TRIGGER_read(void){ 121 return reg_read(SIDE_CH_REG_IQ_TRIGGER_ADDR); 122 } 123 124 static inline void SIDE_CH_REG_IQ_TRIGGER_write(u32 value){ 125 reg_write(SIDE_CH_REG_IQ_TRIGGER_ADDR, value); 126 } 127 128 static inline u32 SIDE_CH_REG_RSSI_TH_read(void){ 129 return reg_read(SIDE_CH_REG_RSSI_TH_ADDR); 130 } 131 132 static inline void SIDE_CH_REG_RSSI_TH_write(u32 value){ 133 reg_write(SIDE_CH_REG_RSSI_TH_ADDR, value); 134 } 135 136 static inline u32 SIDE_CH_REG_GAIN_TH_read(void){ 137 return reg_read(SIDE_CH_REG_GAIN_TH_ADDR); 138 } 139 140 static inline void SIDE_CH_REG_GAIN_TH_write(u32 value){ 141 reg_write(SIDE_CH_REG_GAIN_TH_ADDR, value); 142 } 143 144 static inline u32 SIDE_CH_REG_PRE_TRIGGER_LEN_read(void){ 145 return reg_read(SIDE_CH_REG_PRE_TRIGGER_LEN_ADDR); 146 } 147 148 static inline void SIDE_CH_REG_PRE_TRIGGER_LEN_write(u32 value){ 149 reg_write(SIDE_CH_REG_PRE_TRIGGER_LEN_ADDR, value); 150 } 151 152 static inline u32 SIDE_CH_REG_IQ_LEN_read(void){ 153 return reg_read(SIDE_CH_REG_IQ_LEN_ADDR); 154 } 155 156 static inline void SIDE_CH_REG_IQ_LEN_write(u32 value){ 157 reg_write(SIDE_CH_REG_IQ_LEN_ADDR, value); 158 } 159 160 static inline u32 SIDE_CH_REG_M_AXIS_DATA_COUNT_read(void){ 161 return reg_read(SIDE_CH_REG_M_AXIS_DATA_COUNT_ADDR); 162 } 163 164 static inline void SIDE_CH_REG_M_AXIS_DATA_COUNT_write(u32 value){ 165 reg_write(SIDE_CH_REG_M_AXIS_DATA_COUNT_ADDR, value); 166 } 167 168 static const struct of_device_id dev_of_ids[] = { 169 { .compatible = "sdr,side_ch", }, 170 {} 171 }; 172 MODULE_DEVICE_TABLE(of, dev_of_ids); 173 174 static void chan_to_ps_callback(void *completion) 175 { 176 complete(completion); 177 } 178 179 #if 0 180 static void chan_to_pl_callback(void *completion) 181 { 182 complete(completion); 183 } 184 185 static int dma_loopback_test(int num_test, int num_dma_symbol) { 186 int i, err = 0; 187 188 // -----------dma loop back test------------------------- 189 enum dma_status status; 190 enum dma_ctrl_flags flags; 191 u8 *src_buf, *dst_buf; 192 // int num_dma_symbol = 16; 193 int test_buf_size = num_dma_symbol*8; 194 dma_addr_t src_buf_dma; 195 dma_addr_t dst_buf_dma; 196 struct dma_device *chan_to_pl_dev = chan_to_pl->device; 197 struct dma_device *chan_to_ps_dev = chan_to_ps->device; 198 struct scatterlist chan_to_pl_sg[1]; 199 struct scatterlist chan_to_ps_sg[1]; 200 dma_cookie_t chan_to_pl_cookie; 201 dma_cookie_t chan_to_ps_cookie; 202 struct completion chan_to_pl_cmp; 203 struct completion chan_to_ps_cmp; 204 struct dma_async_tx_descriptor *chan_to_pl_d = NULL; 205 struct dma_async_tx_descriptor *chan_to_ps_d = NULL; 206 unsigned long chan_to_ps_tmo = msecs_to_jiffies(300000); 207 unsigned long chan_to_pl_tmo = msecs_to_jiffies(30000); 208 int test_idx; 209 210 for (test_idx=0; test_idx<num_test; test_idx++) { 211 printk("%s test_idx %d\n", side_ch_compatible_str, test_idx); 212 //set number of dma symbols expected to pl and ps 213 SIDE_CH_REG_NUM_DMA_SYMBOL_write((num_dma_symbol<<16)|num_dma_symbol); 214 215 src_buf = kmalloc(test_buf_size, GFP_KERNEL); 216 if (!src_buf) 217 goto err_src_buf; 218 219 dst_buf = kmalloc(test_buf_size, GFP_KERNEL); 220 if (!dst_buf) 221 goto err_dst_buf; 222 223 // test buf init 224 for (i=0; i<test_buf_size; i++) { 225 src_buf[i] = (test_idx+test_buf_size-i-1); 226 dst_buf[i] = 0; 227 } 228 229 set_user_nice(current, 10); 230 flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT; 231 232 src_buf_dma = dma_map_single(chan_to_pl_dev->dev, src_buf, test_buf_size, DMA_MEM_TO_DEV); 233 if (dma_mapping_error(chan_to_pl_dev->dev, src_buf_dma)) { 234 printk("%s dma_loopback_test WARNING chan_to_pl_dev DMA mapping error\n", side_ch_compatible_str); 235 goto err_src_buf_dma_mapping; 236 } 237 238 dst_buf_dma = dma_map_single(chan_to_ps_dev->dev, dst_buf, test_buf_size, DMA_DEV_TO_MEM); 239 if (dma_mapping_error(chan_to_ps_dev->dev, dst_buf_dma)) { 240 printk("%s dma_loopback_test WARNING chan_to_ps_dev DMA mapping error\n", side_ch_compatible_str); 241 goto err_dst_buf_dma_mapping; 242 } 243 244 sg_init_table(chan_to_ps_sg, 1); 245 sg_init_table(chan_to_pl_sg, 1); 246 247 sg_dma_address(&chan_to_ps_sg[0]) = dst_buf_dma; 248 sg_dma_address(&chan_to_pl_sg[0]) = src_buf_dma; 249 250 sg_dma_len(&chan_to_ps_sg[0]) = test_buf_size; 251 sg_dma_len(&chan_to_pl_sg[0]) = test_buf_size; 252 253 chan_to_ps_d = chan_to_ps_dev->device_prep_slave_sg(chan_to_ps, chan_to_ps_sg, 1, DMA_DEV_TO_MEM, flags, NULL); 254 chan_to_pl_d = chan_to_pl_dev->device_prep_slave_sg(chan_to_pl, chan_to_pl_sg, 1, DMA_MEM_TO_DEV, flags, NULL); 255 256 if (!chan_to_ps_d || !chan_to_pl_d) { 257 printk("%s dma_loopback_test WARNING !chan_to_ps_d || !chan_to_pl_d\n", side_ch_compatible_str); 258 goto err_dst_buf_with_unmap; 259 } 260 261 init_completion(&chan_to_pl_cmp); 262 chan_to_pl_d->callback = chan_to_pl_callback; 263 chan_to_pl_d->callback_param = &chan_to_pl_cmp; 264 chan_to_pl_cookie = chan_to_pl_d->tx_submit(chan_to_pl_d); 265 266 init_completion(&chan_to_ps_cmp); 267 chan_to_ps_d->callback = chan_to_ps_callback; 268 chan_to_ps_d->callback_param = &chan_to_ps_cmp; 269 chan_to_ps_cookie = chan_to_ps_d->tx_submit(chan_to_ps_d); 270 271 if (dma_submit_error(chan_to_pl_cookie) || dma_submit_error(chan_to_ps_cookie)) { 272 printk("%s dma_loopback_test WARNING dma_submit_error\n", side_ch_compatible_str); 273 goto err_dst_buf_with_unmap; 274 } 275 276 dma_async_issue_pending(chan_to_pl); 277 dma_async_issue_pending(chan_to_ps); 278 279 chan_to_pl_tmo = wait_for_completion_timeout(&chan_to_pl_cmp, chan_to_pl_tmo); 280 281 status = dma_async_is_tx_complete(chan_to_pl, chan_to_pl_cookie, NULL, NULL); 282 if (chan_to_pl_tmo == 0) { 283 printk("%s dma_loopback_test chan_to_pl_tmo == 0\n", side_ch_compatible_str); 284 goto err_dst_buf_with_unmap; 285 } else if (status != DMA_COMPLETE) { 286 printk("%s dma_loopback_test chan_to_pl status != DMA_COMPLETE\n", side_ch_compatible_str); 287 goto err_dst_buf_with_unmap; 288 } 289 290 chan_to_ps_tmo = wait_for_completion_timeout(&chan_to_ps_cmp, chan_to_ps_tmo); 291 status = dma_async_is_tx_complete(chan_to_ps, chan_to_ps_cookie, NULL, NULL); 292 if (chan_to_ps_tmo == 0) { 293 printk("%s dma_loopback_test chan_to_ps_tmo == 0\n", side_ch_compatible_str); 294 goto err_dst_buf_with_unmap; 295 } else if (status != DMA_COMPLETE) { 296 printk("%s dma_loopback_test chan_to_ps status != DMA_COMPLETE\n", side_ch_compatible_str); 297 goto err_dst_buf_with_unmap; 298 } 299 300 dma_unmap_single(chan_to_pl_dev->dev, src_buf_dma, test_buf_size, DMA_MEM_TO_DEV); 301 dma_unmap_single(chan_to_ps_dev->dev, dst_buf_dma, test_buf_size, DMA_DEV_TO_MEM); 302 303 // test buf verification 304 for (i=0; i<test_buf_size; i++) { 305 //printk("%d ", dst_buf[i]); 306 if ( dst_buf[i] != ((test_idx+test_buf_size-i-1)%256) ) 307 break; 308 } 309 printk("\n"); 310 printk("%s dma_loopback_test buf verification end idx %d (test_buf_size %d)\n", side_ch_compatible_str, i, test_buf_size); 311 312 kfree(src_buf); 313 kfree(dst_buf); 314 } 315 316 printk("%s dma_loopback_test err %d\n", side_ch_compatible_str, err); 317 return(err); 318 319 err_dst_buf_with_unmap: 320 dma_unmap_single(chan_to_ps_dev->dev, dst_buf_dma, test_buf_size, DMA_DEV_TO_MEM); 321 322 err_dst_buf_dma_mapping: 323 dma_unmap_single(chan_to_pl_dev->dev, src_buf_dma, test_buf_size, DMA_MEM_TO_DEV); 324 325 err_src_buf_dma_mapping: 326 327 err_dst_buf: 328 err = -4; 329 kfree((void*)dst_buf); 330 331 err_src_buf: 332 err = -3; 333 kfree(src_buf); 334 335 return(err); 336 } 337 #endif 338 339 static int init_side_channel(void) { 340 side_info_buf = kmalloc(max_side_info_buf_size, GFP_KERNEL); 341 if (!side_info_buf) 342 return(-1); 343 344 return(0); 345 } 346 347 static int get_side_info(int num_eq, int iq_len) { 348 // int err = 0;//, i; 349 struct scatterlist chan_to_ps_sg[1]; 350 enum dma_status status; 351 enum dma_ctrl_flags flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT; 352 int num_dma_symbol, num_dma_symbol_per_trans, side_info_buf_size; 353 dma_addr_t side_info_buf_dma; 354 struct dma_device *chan_to_ps_dev = chan_to_ps->device; 355 struct completion chan_to_ps_cmp; 356 struct dma_async_tx_descriptor *chan_to_ps_d = NULL; 357 unsigned long chan_to_ps_tmo = msecs_to_jiffies(100); 358 359 if (side_info_buf==NULL) { 360 printk("%s get_side_info WARNING side_info_buf==NULL\n", side_ch_compatible_str); 361 return(-1); 362 } 363 364 status = dma_async_is_tx_complete(chan_to_ps, chan_to_ps_cookie, NULL, NULL); 365 if (status!=DMA_COMPLETE) { 366 printk("%s get_side_info WARNING status!=DMA_COMPLETE\n", side_ch_compatible_str); 367 return(-1); 368 } 369 370 set_user_nice(current, 10); 371 372 if (iq_len>0) 373 num_dma_symbol_per_trans = 1+iq_len; 374 else 375 num_dma_symbol_per_trans = HEADER_LEN + CSI_LEN + num_eq*EQUALIZER_LEN; 376 //set number of dma symbols expected to ps 377 num_dma_symbol = SIDE_CH_REG_M_AXIS_DATA_COUNT_read(); 378 printk("%s get_side_info m axis data count %d per trans %d\n", side_ch_compatible_str, num_dma_symbol, num_dma_symbol_per_trans); 379 num_dma_symbol = num_dma_symbol_per_trans*(num_dma_symbol/num_dma_symbol_per_trans); 380 printk("%s get_side_info actual num dma symbol %d\n", side_ch_compatible_str, num_dma_symbol); 381 if (num_dma_symbol == 0) 382 return(-2); 383 384 side_info_buf_size = num_dma_symbol*8; 385 side_info_buf_dma = dma_map_single(chan_to_ps_dev->dev, side_info_buf, side_info_buf_size, DMA_DEV_TO_MEM); 386 if (dma_mapping_error(chan_to_ps_dev->dev, side_info_buf_dma)) { 387 printk("%s get_side_info WARNING chan_to_ps_dev DMA mapping error\n", side_ch_compatible_str); 388 return(-3); 389 } 390 391 sg_init_table(chan_to_ps_sg, 1); 392 sg_dma_address(&chan_to_ps_sg[0]) = side_info_buf_dma; 393 sg_dma_len(&chan_to_ps_sg[0]) = side_info_buf_size; 394 395 chan_to_ps_d = chan_to_ps_dev->device_prep_slave_sg(chan_to_ps, chan_to_ps_sg, 1, DMA_DEV_TO_MEM, flags, NULL); 396 if (!chan_to_ps_d) { 397 printk("%s get_side_info WARNING !chan_to_ps_d\n", side_ch_compatible_str); 398 goto err_dst_buf_with_unmap; 399 } 400 401 init_completion(&chan_to_ps_cmp); 402 chan_to_ps_d->callback = chan_to_ps_callback; 403 chan_to_ps_d->callback_param = &chan_to_ps_cmp; 404 405 chan_to_ps_cookie = chan_to_ps_d->tx_submit(chan_to_ps_d); 406 if (dma_submit_error(chan_to_ps_cookie)) { 407 printk("%s get_side_info WARNING dma_submit_error\n", side_ch_compatible_str); 408 goto err_dst_buf_with_unmap; 409 } 410 411 SIDE_CH_REG_NUM_DMA_SYMBOL_write(num_dma_symbol); //dma from fpga will start automatically 412 413 dma_async_issue_pending(chan_to_ps); 414 415 chan_to_ps_tmo = wait_for_completion_timeout(&chan_to_ps_cmp, chan_to_ps_tmo); 416 status = dma_async_is_tx_complete(chan_to_ps, chan_to_ps_cookie, NULL, NULL); 417 if (chan_to_ps_tmo == 0) { 418 printk("%s get_side_info WARNING chan_to_ps_tmo == 0\n", side_ch_compatible_str); 419 goto err_dst_buf_with_unmap; 420 } else if (status != DMA_COMPLETE) { 421 printk("%s get_side_info WARNING chan_to_ps status != DMA_COMPLETE\n", side_ch_compatible_str); 422 goto err_dst_buf_with_unmap; 423 } 424 425 dma_unmap_single(chan_to_ps_dev->dev, side_info_buf_dma, side_info_buf_size, DMA_DEV_TO_MEM); 426 return(side_info_buf_size); 427 428 err_dst_buf_with_unmap: 429 dma_unmap_single(chan_to_ps_dev->dev, side_info_buf_dma, side_info_buf_size, DMA_DEV_TO_MEM); 430 return(-100); 431 } 432 433 // -----------------netlink recv and send----------------- 434 // should align with side_ch_ctl.c in user_space 435 #define ACTION_INVALID 0 436 #define ACTION_REG_WRITE 1 437 #define ACTION_REG_READ 2 438 #define ACTION_SIDE_INFO_GET 3 439 440 #define REG_TYPE_INVALID 0 441 #define REG_TYPE_HARDWARE 1 442 #define REG_TYPE_SOFTWARE 2 443 444 // #define NETLINK_USER 31 445 struct sock *nl_sk = NULL; 446 static void side_ch_nl_recv_msg(struct sk_buff *skb) { 447 struct nlmsghdr *nlh; 448 int pid; 449 struct sk_buff *skb_out; 450 int msg_size; 451 int *msg=(int*)side_info_buf; 452 int action_flag, reg_type, reg_idx; 453 u32 reg_val, *cmd_buf; 454 int res; 455 456 // printk(KERN_INFO "Entering: %s\n", __FUNCTION__); 457 458 // msg_size=strlen(msg); 459 460 nlh=(struct nlmsghdr*)skb->data; 461 cmd_buf = (u32*)nlmsg_data(nlh); 462 // printk(KERN_INFO "Netlink received msg payload:%s\n",(char*)nlmsg_data(nlh)); 463 action_flag = cmd_buf[0]; 464 reg_type = cmd_buf[1]; 465 reg_idx = cmd_buf[2]; 466 reg_val = cmd_buf[3]; 467 printk("%s recv msg: len %d action_flag %d reg_type %d reg_idx %d reg_val %u\n", side_ch_compatible_str, nlmsg_len(nlh), action_flag, reg_type, reg_idx, reg_val); 468 469 pid = nlh->nlmsg_pid; /*pid of sending process */ 470 471 if (action_flag==ACTION_SIDE_INFO_GET) { 472 res = get_side_info(num_eq_init, iq_len_init); 473 printk(KERN_INFO "%s recv msg: get_side_info(%d,%d) res %d\n", side_ch_compatible_str, num_eq_init, iq_len_init, res); 474 if (res>0) { 475 msg_size = res; 476 // printk("%s recv msg: %d %d %d %d %d %d %d %d\n", side_ch_compatible_str, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]); 477 } else { 478 msg_size = 4; 479 msg[0] = -2; 480 } 481 } else if (action_flag==ACTION_REG_READ) { 482 msg_size = 4; 483 // if (reg_idx<0 || reg_idx>31) { 484 // msg[0] = -3; 485 // printk("%s recv msg: invalid reg_idx\n", side_ch_compatible_str); 486 // } else { 487 msg[0] = reg_read(reg_idx*4); 488 // } 489 } else if (action_flag==ACTION_REG_WRITE) { 490 msg_size = 4; 491 // if (reg_idx<0 || reg_idx>31) { 492 // msg[0] = -4; 493 // printk("%s recv msg: invalid reg_idx\n", side_ch_compatible_str); 494 // } else { 495 msg[0] = 0; 496 reg_write(reg_idx*4, reg_val); 497 // } 498 } else { 499 msg_size = 4; 500 msg[0] = -1; 501 printk("%s recv msg: invalid action_flag\n", side_ch_compatible_str); 502 } 503 504 skb_out = nlmsg_new(msg_size,0); 505 if(!skb_out) 506 { 507 printk(KERN_ERR "Failed to allocate new skb\n"); 508 return; 509 } 510 nlh=nlmsg_put(skb_out,0,0,NLMSG_DONE,msg_size,0); 511 NETLINK_CB(skb_out).dst_group = 0; /* not in mcast group */ 512 513 memcpy(nlmsg_data(nlh),msg,msg_size); 514 515 res=nlmsg_unicast(nl_sk,skb_out,pid); 516 517 if(res<0) 518 printk(KERN_INFO "Error while sending bak to user\n"); 519 } 520 521 static int dev_probe(struct platform_device *pdev) { 522 struct netlink_kernel_cfg cfg = { 523 .input = side_ch_nl_recv_msg, 524 }; 525 526 struct device_node *np = pdev->dev.of_node; 527 struct resource *io; 528 int err=1, i; 529 530 printk("\n"); 531 532 if (np) { 533 const struct of_device_id *match; 534 535 match = of_match_node(dev_of_ids, np); 536 if (match) { 537 printk("%s dev_probe: match!\n", side_ch_compatible_str); 538 err = 0; 539 } 540 } 541 542 if (err) 543 return err; 544 545 /* Request and map I/O memory */ 546 io = platform_get_resource(pdev, IORESOURCE_MEM, 0); 547 base_addr = devm_ioremap_resource(&pdev->dev, io); 548 if (IS_ERR(base_addr)) 549 return PTR_ERR(base_addr); 550 551 printk("%s dev_probe: io start 0x%p end 0x%p name %s flags 0x%08x desc %s\n", side_ch_compatible_str, (void*)io->start, (void*)io->end, io->name, (u32)io->flags, (char*)io->desc); 552 printk("%s dev_probe: base_addr 0x%p\n", side_ch_compatible_str, base_addr); 553 554 printk("%s dev_probe: succeed!\n", side_ch_compatible_str); 555 556 // --------------initialize netlink-------------- 557 //nl_sk = netlink_kernel_create(&init_net, NETLINK_USER, &cfg); 558 nl_sk = netlink_kernel_create(&init_net, NETLINK_USERSOCK, &cfg); 559 if(!nl_sk) { 560 printk(KERN_ALERT "%s dev_probe: Error creating socket.\n", side_ch_compatible_str); 561 return -10; 562 } 563 564 //-----------------initialize fpga---------------- 565 printk("%s dev_probe: num_eq_init %d iq_len_init %d\n",side_ch_compatible_str, num_eq_init, iq_len_init); 566 567 // disable potential any action from side channel 568 SIDE_CH_REG_MULTI_RST_write(4); 569 // SIDE_CH_REG_CONFIG_write(0X6001); // match addr1 and addr2; bit12 FC; bit13 addr1; bit14 addr2 570 SIDE_CH_REG_CONFIG_write(0x7001); // the most strict condition to prevent side channel action 571 SIDE_CH_REG_IQ_TRIGGER_write(10); // set iq trigger to rssi, which will never happen when rssi_th is 0 572 SIDE_CH_REG_NUM_EQ_write(num_eq_init); // capture CSI + 8*equalizer by default 573 if (iq_len_init>0) {//initialize the side channel into iq capture mode 574 //Max UDP 65507 bytes; (65507/8)-1 = 8187 575 if (iq_len_init>8187) { 576 iq_len_init = 8187; 577 printk("%s dev_probe: limit iq_len_init to 8187!\n",side_ch_compatible_str); 578 } 579 SIDE_CH_REG_IQ_CAPTURE_write(1); 580 SIDE_CH_REG_PRE_TRIGGER_LEN_write(8190); 581 SIDE_CH_REG_IQ_LEN_write(iq_len_init); 582 SIDE_CH_REG_IQ_TRIGGER_write(0); // trigger is set to fcs ok/nok (both) 583 } 584 585 SIDE_CH_REG_CONFIG_write(0x0001); // allow all packets by default; bit12 FC; bit13 addr1; bit14 addr2 586 587 //rst 588 for (i=0;i<8;i++) 589 SIDE_CH_REG_MULTI_RST_write(0); 590 for (i=0;i<32;i++) 591 SIDE_CH_REG_MULTI_RST_write(0xFFFFFFFF); 592 for (i=0;i<8;i++) 593 SIDE_CH_REG_MULTI_RST_write(0); 594 595 // chan_to_pl = dma_request_slave_channel(&(pdev->dev), "rx_dma_mm2s"); 596 // if (IS_ERR(chan_to_pl)) { 597 // err = PTR_ERR(chan_to_pl); 598 // pr_err("%s dev_probe: No channel to PL. %d\n",side_ch_compatible_str,err); 599 // goto free_chan_to_pl; 600 // } 601 602 chan_to_ps = dma_request_chan(&(pdev->dev), "tx_dma_s2mm"); 603 if (IS_ERR(chan_to_ps) || chan_to_ps==NULL) { 604 err = PTR_ERR(chan_to_ps); 605 if (err != -EPROBE_DEFER) { 606 pr_err("%s dev_probe: No chan_to_ps ret %d chan_to_ps 0x%p\n",side_ch_compatible_str, err, chan_to_ps); 607 goto free_chan_to_ps; 608 } 609 } 610 611 printk("%s dev_probe: DMA channel setup successfully. chan_to_pl 0x%p chan_to_ps 0x%p\n",side_ch_compatible_str, chan_to_pl, chan_to_ps); 612 613 // res = dma_loopback_test(3, 512); 614 // printk(KERN_INFO "dma_loopback_test(3, 512) res %d\n", res); 615 616 err = init_side_channel(); 617 printk("%s dev_probe: init_side_channel() err %d\n",side_ch_compatible_str, err); 618 619 return(err); 620 621 // err = dma_loopback_test(7, 512); 622 // if (err == 0) 623 // return(err); 624 // else 625 // dma_release_channel(chan_to_ps); 626 627 free_chan_to_ps: 628 err = -2; 629 dma_release_channel(chan_to_ps); 630 return err; 631 632 // free_chan_to_pl: 633 // err = -1; 634 // dma_release_channel(chan_to_pl); 635 // return err; 636 } 637 638 static int dev_remove(struct platform_device *pdev) 639 { 640 printk("\n"); 641 642 printk("%s dev_remove: release nl_sk\n", side_ch_compatible_str); 643 netlink_kernel_release(nl_sk); 644 645 pr_info("%s dev_remove: dropped chan_to_pl 0x%p\n", side_ch_compatible_str, chan_to_pl); 646 if (chan_to_pl != NULL) { 647 pr_info("%s dev_remove: dropped channel %s\n", side_ch_compatible_str, dma_chan_name(chan_to_pl)); 648 // dmaengine_terminate_all(chan_to_pl); //this also terminate sdr.ko. do not use 649 dma_release_channel(chan_to_pl); 650 } 651 652 pr_info("%s dev_remove: dropped chan_to_ps 0x%p\n", side_ch_compatible_str, chan_to_ps); 653 if (chan_to_pl != NULL) { 654 pr_info("%s dev_remove: dropped channel %s\n", side_ch_compatible_str, dma_chan_name(chan_to_ps)); 655 // dmaengine_terminate_all(chan_to_ps); //this also terminate sdr.ko. do not use 656 dma_release_channel(chan_to_ps); 657 } 658 659 if (side_info_buf != NULL) 660 kfree(side_info_buf); 661 662 printk("%s dev_remove: base_addr 0x%p\n", side_ch_compatible_str, base_addr); 663 printk("%s dev_remove: succeed!\n", side_ch_compatible_str); 664 return 0; 665 } 666 667 static struct platform_driver dev_driver = { 668 .driver = { 669 .name = "sdr,side_ch", 670 .owner = THIS_MODULE, 671 .of_match_table = dev_of_ids, 672 }, 673 .probe = dev_probe, 674 .remove = dev_remove, 675 }; 676 677 module_platform_driver(dev_driver); 678 679 MODULE_AUTHOR("Xianjun Jiao"); 680 MODULE_DESCRIPTION("sdr,side_ch"); 681 MODULE_LICENSE("GPL v2"); 682