1 | /* |
---|
2 | * B1553RT driver implmenetation |
---|
3 | * |
---|
4 | * COPYRIGHT (c) 2009. |
---|
5 | * Cobham Gaisler AB. |
---|
6 | * |
---|
7 | * The license and distribution terms for this file may be |
---|
8 | * found in the file LICENSE in this distribution or at |
---|
9 | * http://www.rtems.org/license/LICENSE. |
---|
10 | */ |
---|
11 | |
---|
12 | #include <bsp.h> |
---|
13 | #include <rtems/libio.h> |
---|
14 | #include <stdlib.h> |
---|
15 | #include <stdio.h> |
---|
16 | #include <string.h> |
---|
17 | #include <assert.h> |
---|
18 | #include <ctype.h> |
---|
19 | #include <rtems/bspIo.h> |
---|
20 | |
---|
21 | #include <drvmgr/drvmgr.h> |
---|
22 | #include <bsp/b1553rt.h> |
---|
23 | #include <ambapp.h> |
---|
24 | #include <drvmgr/ambapp_bus.h> |
---|
25 | |
---|
26 | /* Uncomment for debug output */ |
---|
27 | /*#define DEBUG 1*/ |
---|
28 | |
---|
29 | /* |
---|
30 | #define FUNCDEBUG 1*/ |
---|
31 | /*#undef DEBUG*/ |
---|
32 | #undef FUNCDEBUG |
---|
33 | |
---|
34 | /* EVENT_QUEUE_SIZE sets the size of the event queue |
---|
35 | */ |
---|
36 | #define EVENT_QUEUE_SIZE 1024 |
---|
37 | |
---|
38 | |
---|
39 | #define INDEX(x) ( x&(EVENT_QUEUE_SIZE-1) ) |
---|
40 | |
---|
41 | #if 0 |
---|
42 | #define DBG(x...) printk(x) |
---|
43 | #else |
---|
44 | #define DBG(x...) |
---|
45 | #endif |
---|
46 | |
---|
47 | #ifdef FUNCDEBUG |
---|
48 | #define FUNCDBG(x...) printk(x) |
---|
49 | #else |
---|
50 | #define FUNCDBG(x...) |
---|
51 | #endif |
---|
52 | |
---|
53 | #define READ_DMA(address) _READ16((unsigned int)address) |
---|
54 | |
---|
55 | static __inline__ unsigned short _READ16(unsigned int addr) { |
---|
56 | unsigned short tmp; |
---|
57 | asm(" lduha [%1]1, %0 " |
---|
58 | : "=r"(tmp) |
---|
59 | : "r"(addr) |
---|
60 | ); |
---|
61 | return tmp; |
---|
62 | } |
---|
63 | |
---|
64 | static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
65 | static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
66 | static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
67 | static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
68 | static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
69 | static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); |
---|
70 | |
---|
71 | #define RT_DRIVER_TABLE_ENTRY { rt_initialize, rt_open, rt_close, rt_read, rt_write, rt_control } |
---|
72 | |
---|
73 | static rtems_driver_address_table b1553rt_driver = RT_DRIVER_TABLE_ENTRY; |
---|
74 | |
---|
75 | typedef struct { |
---|
76 | |
---|
77 | struct drvmgr_dev *dev; /* Driver manager device */ |
---|
78 | char devName[32]; /* Device Name */ |
---|
79 | |
---|
80 | struct rt_reg *regs; |
---|
81 | unsigned int ctrl_copy; /* Local copy of config register */ |
---|
82 | |
---|
83 | unsigned int cfg_freq; |
---|
84 | |
---|
85 | unsigned int memarea_base; |
---|
86 | unsigned int memarea_base_remote; |
---|
87 | |
---|
88 | volatile unsigned short *mem; |
---|
89 | |
---|
90 | /* Received events waiting to be read */ |
---|
91 | struct rt_msg *rt_event; |
---|
92 | unsigned int head, tail; |
---|
93 | |
---|
94 | int rx_blocking; |
---|
95 | |
---|
96 | rtems_id rx_sem, tx_sem, dev_sem; |
---|
97 | int minor; |
---|
98 | int irqno; |
---|
99 | |
---|
100 | #ifdef DEBUG |
---|
101 | unsigned int log[EVENT_QUEUE_SIZE*4]; |
---|
102 | unsigned int log_i; |
---|
103 | #endif |
---|
104 | |
---|
105 | unsigned int status; |
---|
106 | rtems_id event_id; /* event that may be signalled upon errors, needs to be set through ioctl command RT_SET_EVENTID */ |
---|
107 | |
---|
108 | } rt_priv; |
---|
109 | |
---|
110 | static void b1553rt_interrupt(void *arg); |
---|
111 | static rtems_device_driver rt_init(rt_priv *rt); |
---|
112 | |
---|
113 | #define OFS(ofs) (((unsigned int)&ofs & 0x1ffff)>>1) |
---|
114 | |
---|
115 | static int b1553rt_driver_io_registered = 0; |
---|
116 | static rtems_device_major_number b1553rt_driver_io_major = 0; |
---|
117 | |
---|
118 | /******************* Driver manager interface ***********************/ |
---|
119 | |
---|
120 | /* Driver prototypes */ |
---|
121 | int b1553rt_register_io(rtems_device_major_number *m); |
---|
122 | int b1553rt_device_init(rt_priv *pDev); |
---|
123 | |
---|
124 | int b1553rt_init2(struct drvmgr_dev *dev); |
---|
125 | int b1553rt_init3(struct drvmgr_dev *dev); |
---|
126 | int b1553rt_remove(struct drvmgr_dev *dev); |
---|
127 | |
---|
128 | struct drvmgr_drv_ops b1553rt_ops = |
---|
129 | { |
---|
130 | .init = {NULL, b1553rt_init2, b1553rt_init3, NULL}, |
---|
131 | .remove = b1553rt_remove, |
---|
132 | .info = NULL |
---|
133 | }; |
---|
134 | |
---|
135 | struct amba_dev_id b1553rt_ids[] = |
---|
136 | { |
---|
137 | {VENDOR_GAISLER, GAISLER_B1553RT}, |
---|
138 | {0, 0} /* Mark end of table */ |
---|
139 | }; |
---|
140 | |
---|
141 | struct amba_drv_info b1553rt_drv_info = |
---|
142 | { |
---|
143 | { |
---|
144 | DRVMGR_OBJ_DRV, /* Driver */ |
---|
145 | NULL, /* Next driver */ |
---|
146 | NULL, /* Device list */ |
---|
147 | DRIVER_AMBAPP_GAISLER_B1553RT_ID, /* Driver ID */ |
---|
148 | "B1553RT_DRV", /* Driver Name */ |
---|
149 | DRVMGR_BUS_TYPE_AMBAPP, /* Bus Type */ |
---|
150 | &b1553rt_ops, |
---|
151 | NULL, /* Funcs */ |
---|
152 | 0, /* No devices yet */ |
---|
153 | 0, |
---|
154 | |
---|
155 | }, |
---|
156 | &b1553rt_ids[0] |
---|
157 | }; |
---|
158 | |
---|
159 | void b1553rt_register_drv (void) |
---|
160 | { |
---|
161 | DBG("Registering B1553RT driver\n"); |
---|
162 | drvmgr_drv_register(&b1553rt_drv_info.general); |
---|
163 | } |
---|
164 | |
---|
165 | int b1553rt_init2(struct drvmgr_dev *dev) |
---|
166 | { |
---|
167 | rt_priv *priv; |
---|
168 | |
---|
169 | DBG("B1553RT[%d] on bus %s\n", dev->minor_drv, dev->parent->dev->name); |
---|
170 | priv = dev->priv = malloc(sizeof(rt_priv)); |
---|
171 | if ( !priv ) |
---|
172 | return DRVMGR_NOMEM; |
---|
173 | memset(priv, 0, sizeof(*priv)); |
---|
174 | priv->dev = dev; |
---|
175 | |
---|
176 | /* This core will not find other cores, so we wait for init2() */ |
---|
177 | |
---|
178 | return DRVMGR_OK; |
---|
179 | } |
---|
180 | |
---|
181 | int b1553rt_init3(struct drvmgr_dev *dev) |
---|
182 | { |
---|
183 | rt_priv *priv; |
---|
184 | char prefix[32]; |
---|
185 | rtems_status_code status; |
---|
186 | |
---|
187 | priv = dev->priv; |
---|
188 | |
---|
189 | /* Do initialization */ |
---|
190 | |
---|
191 | if ( b1553rt_driver_io_registered == 0) { |
---|
192 | /* Register the I/O driver only once for all cores */ |
---|
193 | if ( b1553rt_register_io(&b1553rt_driver_io_major) ) { |
---|
194 | /* Failed to register I/O driver */ |
---|
195 | dev->priv = NULL; |
---|
196 | return DRVMGR_FAIL; |
---|
197 | } |
---|
198 | |
---|
199 | b1553rt_driver_io_registered = 1; |
---|
200 | } |
---|
201 | |
---|
202 | /* I/O system registered and initialized |
---|
203 | * Now we take care of device initialization. |
---|
204 | */ |
---|
205 | |
---|
206 | if ( b1553rt_device_init(priv) ) { |
---|
207 | return DRVMGR_FAIL; |
---|
208 | } |
---|
209 | |
---|
210 | /* Get Filesystem name prefix */ |
---|
211 | prefix[0] = '\0'; |
---|
212 | if ( drvmgr_get_dev_prefix(dev, prefix) ) { |
---|
213 | /* Failed to get prefix, make sure of a unique FS name |
---|
214 | * by using the driver minor. |
---|
215 | */ |
---|
216 | sprintf(priv->devName, "/dev/b1553rt%d", dev->minor_drv); |
---|
217 | } else { |
---|
218 | /* Got special prefix, this means we have a bus prefix |
---|
219 | * And we should use our "bus minor" |
---|
220 | */ |
---|
221 | sprintf(priv->devName, "/dev/%sb1553rt%d", prefix, dev->minor_bus); |
---|
222 | } |
---|
223 | |
---|
224 | /* Register Device */ |
---|
225 | status = rtems_io_register_name(priv->devName, b1553rt_driver_io_major, dev->minor_drv); |
---|
226 | if (status != RTEMS_SUCCESSFUL) { |
---|
227 | return DRVMGR_FAIL; |
---|
228 | } |
---|
229 | |
---|
230 | return DRVMGR_OK; |
---|
231 | } |
---|
232 | |
---|
233 | int b1553rt_remove(struct drvmgr_dev *dev) |
---|
234 | { |
---|
235 | /* Stop more tasks to open driver */ |
---|
236 | |
---|
237 | /* Throw out all tasks using this driver */ |
---|
238 | |
---|
239 | /* Unregister I/O node */ |
---|
240 | |
---|
241 | /* Unregister and disable Interrupt */ |
---|
242 | |
---|
243 | /* Free device memory */ |
---|
244 | |
---|
245 | /* Return sucessfully */ |
---|
246 | |
---|
247 | return DRVMGR_FAIL; |
---|
248 | } |
---|
249 | |
---|
250 | /******************* Driver Implementation ***********************/ |
---|
251 | |
---|
252 | int b1553rt_register_io(rtems_device_major_number *m) |
---|
253 | { |
---|
254 | rtems_status_code r; |
---|
255 | |
---|
256 | if ((r = rtems_io_register_driver(0, &b1553rt_driver, m)) == RTEMS_SUCCESSFUL) { |
---|
257 | DBG("B1553RT driver successfully registered, major: %d\n", *m); |
---|
258 | } else { |
---|
259 | switch(r) { |
---|
260 | case RTEMS_TOO_MANY: |
---|
261 | printk("B1553RT rtems_io_register_driver failed: RTEMS_TOO_MANY\n"); |
---|
262 | return -1; |
---|
263 | case RTEMS_INVALID_NUMBER: |
---|
264 | printk("B1553RT rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n"); |
---|
265 | return -1; |
---|
266 | case RTEMS_RESOURCE_IN_USE: |
---|
267 | printk("B1553RT rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n"); |
---|
268 | return -1; |
---|
269 | default: |
---|
270 | printk("B1553RT rtems_io_register_driver failed\n"); |
---|
271 | return -1; |
---|
272 | } |
---|
273 | } |
---|
274 | return 0; |
---|
275 | } |
---|
276 | |
---|
277 | int b1553rt_device_init(rt_priv *pDev) |
---|
278 | { |
---|
279 | struct amba_dev_info *ambadev; |
---|
280 | struct ambapp_core *pnpinfo; |
---|
281 | union drvmgr_key_value *value; |
---|
282 | unsigned int mem; |
---|
283 | unsigned int sys_freq_hz; |
---|
284 | |
---|
285 | /* Get device information from AMBA PnP information */ |
---|
286 | ambadev = (struct amba_dev_info *)pDev->dev->businfo; |
---|
287 | if ( ambadev == NULL ) { |
---|
288 | return -1; |
---|
289 | } |
---|
290 | pnpinfo = &ambadev->info; |
---|
291 | pDev->irqno = pnpinfo->irq; |
---|
292 | pDev->regs = (struct rt_reg *)pnpinfo->apb_slv->start; |
---|
293 | pDev->minor = pDev->dev->minor_drv; |
---|
294 | |
---|
295 | #ifdef DEBUG |
---|
296 | pDev->log_i = 0; |
---|
297 | memset(pDev->log,0,sizeof(pDev->log)); |
---|
298 | printf("LOG: 0x%x\n", &pDev->log[0]); |
---|
299 | printf("LOG_I: 0x%x\n", &pDev->log_i); |
---|
300 | #endif |
---|
301 | |
---|
302 | /* Get memory configuration from bus resources */ |
---|
303 | value = drvmgr_dev_key_get(pDev->dev, "dmaBaseAdr", DRVMGR_KT_POINTER); |
---|
304 | if (value) |
---|
305 | mem = (unsigned int)value->ptr; |
---|
306 | |
---|
307 | if (value && (mem & 1)) { |
---|
308 | /* Remote address, address as RT looks at it. */ |
---|
309 | |
---|
310 | /* Translate the base address into an address that the the CPU can understand */ |
---|
311 | pDev->memarea_base = mem & ~1; |
---|
312 | drvmgr_translate_check(pDev->dev, DMAMEM_TO_CPU, |
---|
313 | (void *)pDev->memarea_base_remote, |
---|
314 | (void **)&pDev->memarea_base, |
---|
315 | 4 * 1024); |
---|
316 | } else { |
---|
317 | if (!value) { |
---|
318 | /* Use dynamically allocated memory, |
---|
319 | * 4k DMA memory + 4k for alignment |
---|
320 | */ |
---|
321 | mem = (unsigned int)malloc(4 * 1024 * 2); |
---|
322 | if ( !mem ){ |
---|
323 | printk("RT: Failed to allocate HW memory\n\r"); |
---|
324 | return -1; |
---|
325 | } |
---|
326 | /* align memory to 4k boundary */ |
---|
327 | pDev->memarea_base = (mem + 0xfff) & ~0xfff; |
---|
328 | } else { |
---|
329 | pDev->memarea_base = mem; |
---|
330 | } |
---|
331 | |
---|
332 | /* Translate the base address into an address that the RT core can understand */ |
---|
333 | drvmgr_translate_check(pDev->dev, CPUMEM_TO_DMA, |
---|
334 | (void *)pDev->memarea_base, |
---|
335 | (void **)&pDev->memarea_base_remote, |
---|
336 | 4 * 1024); |
---|
337 | } |
---|
338 | |
---|
339 | /* clear the used memory */ |
---|
340 | memset((char *)pDev->memarea_base, 0, 4 * 1024); |
---|
341 | |
---|
342 | /* Set base address of all descriptors */ |
---|
343 | pDev->memarea_base = (unsigned int)mem; |
---|
344 | pDev->mem = (volatile unsigned short *)pDev->memarea_base; |
---|
345 | |
---|
346 | pDev->rt_event = NULL; |
---|
347 | |
---|
348 | /* The RT is always clocked at the same frequency as the bus |
---|
349 | * If the frequency doesnt match it is defaulted to 24MHz, |
---|
350 | * user can always override it. |
---|
351 | */ |
---|
352 | pDev->cfg_freq = RT_FREQ_24MHZ; |
---|
353 | |
---|
354 | /* Get frequency in Hz */ |
---|
355 | if ( drvmgr_freq_get(pDev->dev, DEV_APB_SLV, &sys_freq_hz) == 0 ) { |
---|
356 | if ( sys_freq_hz == 20000000 ) { |
---|
357 | pDev->cfg_freq = RT_FREQ_20MHZ; |
---|
358 | } else if ( sys_freq_hz == 16000000 ) { |
---|
359 | pDev->cfg_freq = RT_FREQ_16MHZ; |
---|
360 | } else if ( sys_freq_hz == 12000000 ) { |
---|
361 | pDev->cfg_freq = RT_FREQ_12MHZ; |
---|
362 | } |
---|
363 | } |
---|
364 | |
---|
365 | value = drvmgr_dev_key_get(pDev->dev, "coreFreq", DRVMGR_KT_INT); |
---|
366 | if ( value ) { |
---|
367 | pDev->cfg_freq = value->i & RT_FREQ_MASK; |
---|
368 | } |
---|
369 | |
---|
370 | /* RX Semaphore created with count = 0 */ |
---|
371 | if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor), |
---|
372 | 0, |
---|
373 | RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, |
---|
374 | 0, |
---|
375 | &pDev->rx_sem) != RTEMS_SUCCESSFUL ) { |
---|
376 | printk("RT: Failed to create rx semaphore\n"); |
---|
377 | return RTEMS_INTERNAL_ERROR; |
---|
378 | } |
---|
379 | |
---|
380 | /* Device Semaphore created with count = 1 */ |
---|
381 | if ( rtems_semaphore_create(rtems_build_name('R', 'T', '0', '0' + pDev->minor), |
---|
382 | 1, |
---|
383 | RTEMS_FIFO|RTEMS_SIMPLE_BINARY_SEMAPHORE|RTEMS_NO_INHERIT_PRIORITY|RTEMS_LOCAL|RTEMS_NO_PRIORITY_CEILING, |
---|
384 | 0, |
---|
385 | &pDev->dev_sem) != RTEMS_SUCCESSFUL ){ |
---|
386 | printk("RT: Failed to create device semaphore\n"); |
---|
387 | return RTEMS_INTERNAL_ERROR; |
---|
388 | } |
---|
389 | |
---|
390 | /* Default to RT-mode */ |
---|
391 | rt_init(pDev); |
---|
392 | |
---|
393 | return 0; |
---|
394 | } |
---|
395 | |
---|
396 | static int odd_parity(unsigned int data) |
---|
397 | { |
---|
398 | unsigned int i=0; |
---|
399 | |
---|
400 | while(data) |
---|
401 | { |
---|
402 | i++; |
---|
403 | data &= (data - 1); |
---|
404 | } |
---|
405 | |
---|
406 | return !(i&1); |
---|
407 | } |
---|
408 | |
---|
409 | static void start_operation(rt_priv *rt) |
---|
410 | { |
---|
411 | |
---|
412 | } |
---|
413 | |
---|
414 | static void stop_operation(rt_priv *rt) |
---|
415 | { |
---|
416 | |
---|
417 | } |
---|
418 | |
---|
419 | static void set_extmdata_en(rt_priv *rt, int extmdata) |
---|
420 | { |
---|
421 | if ( extmdata ) |
---|
422 | extmdata = 1; |
---|
423 | rt->ctrl_copy = (rt->ctrl_copy & ~(1<<16)) | (extmdata<<16); |
---|
424 | rt->regs->ctrl = rt->ctrl_copy; |
---|
425 | } |
---|
426 | |
---|
427 | static void set_vector_word(rt_priv *rt, unsigned short vword) |
---|
428 | { |
---|
429 | rt->regs->vword = vword; |
---|
430 | } |
---|
431 | |
---|
432 | /* Set clock speed */ |
---|
433 | static void set_clkspd(rt_priv *rt, int spd) |
---|
434 | { |
---|
435 | rt->ctrl_copy = (rt->ctrl_copy & ~0xC0) | (spd<<6); |
---|
436 | rt->regs->ctrl = rt->ctrl_copy; |
---|
437 | asm volatile("nop"::); |
---|
438 | rt->regs->ctrl = rt->ctrl_copy | (1<<20); |
---|
439 | } |
---|
440 | |
---|
441 | static void set_rtaddr(rt_priv *rt, int addr) |
---|
442 | { |
---|
443 | rt->ctrl_copy = (rt->ctrl_copy & ~0x3F00) | (addr << 8) | (odd_parity(addr)<<13); |
---|
444 | rt->regs->ctrl = rt->ctrl_copy; |
---|
445 | } |
---|
446 | |
---|
447 | static void set_broadcast_en(rt_priv *rt, int data) |
---|
448 | { |
---|
449 | rt->ctrl_copy = (rt->ctrl_copy & ~0x40000) | (data<<18); |
---|
450 | rt->regs->ctrl = rt->ctrl_copy; |
---|
451 | } |
---|
452 | |
---|
453 | static rtems_device_driver rt_init(rt_priv *rt) |
---|
454 | { |
---|
455 | rt->rx_blocking = 1; |
---|
456 | |
---|
457 | if ( rt->rt_event ) |
---|
458 | free(rt->rt_event); |
---|
459 | rt->rt_event = NULL; |
---|
460 | |
---|
461 | rt->rt_event = (struct rt_msg *) malloc(EVENT_QUEUE_SIZE*sizeof(struct rt_msg)); |
---|
462 | |
---|
463 | if (rt->rt_event == NULL) { |
---|
464 | DBG("RT driver failed to allocated memory."); |
---|
465 | return RTEMS_NO_MEMORY; |
---|
466 | } |
---|
467 | |
---|
468 | rt->ctrl_copy = rt->regs->ctrl & 0x3F00; /* Keep rtaddr and rtaddrp */ |
---|
469 | rt->ctrl_copy |= 0x3C0D0; /* broadcast disabled, extmdata=1, writetsw = writecmd = 1 */ |
---|
470 | rt->regs->ctrl = rt->ctrl_copy; |
---|
471 | |
---|
472 | /* Set Clock speed */ |
---|
473 | set_clkspd(rt, rt->cfg_freq); |
---|
474 | |
---|
475 | rt->regs->addr = rt->memarea_base_remote; |
---|
476 | rt->regs->ipm = 0x70000; /* Enable RT RX, MEM Failure and AHB Error interrupts */ |
---|
477 | |
---|
478 | DBG("B1553RT DMA_AREA: 0x%x\n", (unsigned int)rt->mem); |
---|
479 | |
---|
480 | return RTEMS_SUCCESSFUL; |
---|
481 | } |
---|
482 | |
---|
483 | |
---|
484 | static rtems_device_driver rt_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) |
---|
485 | { |
---|
486 | return RTEMS_SUCCESSFUL; |
---|
487 | } |
---|
488 | |
---|
489 | static rtems_device_driver rt_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) { |
---|
490 | rt_priv *rt; |
---|
491 | struct drvmgr_dev *dev; |
---|
492 | |
---|
493 | FUNCDBG("rt_open\n"); |
---|
494 | |
---|
495 | if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) { |
---|
496 | DBG("Wrong minor %d\n", minor); |
---|
497 | return RTEMS_UNSATISFIED; |
---|
498 | } |
---|
499 | rt = (rt_priv *)dev->priv; |
---|
500 | |
---|
501 | if (rtems_semaphore_obtain(rt->dev_sem, RTEMS_NO_WAIT, RTEMS_NO_TIMEOUT) != RTEMS_SUCCESSFUL) { |
---|
502 | DBG("rt_open: resource in use\n"); |
---|
503 | return RTEMS_RESOURCE_IN_USE; /* EBUSY */ |
---|
504 | } |
---|
505 | |
---|
506 | /* Set defaults */ |
---|
507 | rt->event_id = 0; |
---|
508 | |
---|
509 | start_operation(rt); |
---|
510 | |
---|
511 | /* Register interrupt routine */ |
---|
512 | if (drvmgr_interrupt_register(rt->dev, 0, "b1553rt", b1553rt_interrupt, rt)) { |
---|
513 | rtems_semaphore_release(rt->dev_sem); |
---|
514 | return -1; |
---|
515 | } |
---|
516 | |
---|
517 | |
---|
518 | return RTEMS_SUCCESSFUL; |
---|
519 | } |
---|
520 | |
---|
521 | static rtems_device_driver rt_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) |
---|
522 | { |
---|
523 | rt_priv *rt; |
---|
524 | struct drvmgr_dev *dev; |
---|
525 | |
---|
526 | FUNCDBG("rt_close"); |
---|
527 | |
---|
528 | if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) { |
---|
529 | return RTEMS_UNSATISFIED; |
---|
530 | } |
---|
531 | rt = (rt_priv *)dev->priv; |
---|
532 | |
---|
533 | drvmgr_interrupt_unregister(rt->dev, 0, b1553rt_interrupt, rt); |
---|
534 | |
---|
535 | stop_operation(rt); |
---|
536 | rtems_semaphore_release(rt->dev_sem); |
---|
537 | |
---|
538 | return RTEMS_SUCCESSFUL; |
---|
539 | } |
---|
540 | |
---|
541 | static int get_messages(rt_priv *rt, void *buf, unsigned int msg_count) |
---|
542 | { |
---|
543 | |
---|
544 | struct rt_msg *dest = (struct rt_msg *) buf; |
---|
545 | int count = 0; |
---|
546 | |
---|
547 | if (rt->head == rt->tail) { |
---|
548 | return 0; |
---|
549 | } |
---|
550 | |
---|
551 | do { |
---|
552 | |
---|
553 | DBG("rt read - head: %d, tail: %d\n", rt->head, rt->tail); |
---|
554 | dest[count++] = rt->rt_event[INDEX(rt->tail++)]; |
---|
555 | |
---|
556 | } while (rt->head != rt->tail && count < msg_count); |
---|
557 | |
---|
558 | return count; |
---|
559 | |
---|
560 | } |
---|
561 | static rtems_device_driver rt_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) |
---|
562 | { |
---|
563 | rtems_libio_rw_args_t *rw_args; |
---|
564 | int count = 0; |
---|
565 | rt_priv *rt; |
---|
566 | struct drvmgr_dev *dev; |
---|
567 | |
---|
568 | if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) { |
---|
569 | return RTEMS_UNSATISFIED; |
---|
570 | } |
---|
571 | rt = (rt_priv *)dev->priv; |
---|
572 | |
---|
573 | rw_args = (rtems_libio_rw_args_t *) arg; |
---|
574 | |
---|
575 | FUNCDBG("rt_read [%i,%i]: buf: 0x%x, len: %i\n",major, minor, (unsigned int)rw_args->buffer, rw_args->count); |
---|
576 | |
---|
577 | while ( (count = get_messages(rt,rw_args->buffer, rw_args->count)) == 0 ) { |
---|
578 | |
---|
579 | if (rt->rx_blocking) { |
---|
580 | rtems_semaphore_obtain(rt->rx_sem, RTEMS_WAIT, RTEMS_NO_TIMEOUT); |
---|
581 | } else { |
---|
582 | /* Translates to EBUSY */ |
---|
583 | return RTEMS_RESOURCE_IN_USE; |
---|
584 | } |
---|
585 | } |
---|
586 | |
---|
587 | rw_args->bytes_moved = count; |
---|
588 | return RTEMS_SUCCESSFUL; |
---|
589 | } |
---|
590 | |
---|
591 | static rtems_device_driver rt_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) |
---|
592 | { |
---|
593 | rtems_libio_rw_args_t *rw_args; |
---|
594 | struct rt_msg *source; |
---|
595 | rt_priv *rt; |
---|
596 | struct drvmgr_dev *dev; |
---|
597 | unsigned int descriptor, suba, wc; |
---|
598 | |
---|
599 | if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) { |
---|
600 | return RTEMS_UNSATISFIED; |
---|
601 | } |
---|
602 | rt = (rt_priv *)dev->priv; |
---|
603 | |
---|
604 | rw_args = (rtems_libio_rw_args_t *) arg; |
---|
605 | |
---|
606 | if ( rw_args->count != 1 ) { |
---|
607 | return RTEMS_INVALID_NAME; |
---|
608 | } |
---|
609 | |
---|
610 | source = (struct rt_msg *) rw_args->buffer; |
---|
611 | |
---|
612 | descriptor = source[0].desc & 0x7F; |
---|
613 | suba = descriptor-32; |
---|
614 | wc = source[0].miw >> 11; |
---|
615 | wc = wc ? wc : 32; |
---|
616 | |
---|
617 | FUNCDBG("rt_write [%i,%i]: buf: 0x%x\n",major, minor, (unsigned int)rw_args->buffer); |
---|
618 | |
---|
619 | memcpy((void *)&rt->mem[0x400 + suba*32], &source[0].data[0], wc*2); |
---|
620 | |
---|
621 | rw_args->bytes_moved = 1; |
---|
622 | |
---|
623 | return RTEMS_SUCCESSFUL; |
---|
624 | |
---|
625 | } |
---|
626 | |
---|
627 | static rtems_device_driver rt_control(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) |
---|
628 | { |
---|
629 | rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t *) arg; |
---|
630 | unsigned int *data = ioarg->buffer; |
---|
631 | |
---|
632 | rt_priv *rt; |
---|
633 | struct drvmgr_dev *dev; |
---|
634 | |
---|
635 | FUNCDBG("rt_control[%d]: [%i,%i]\n", minor, major, minor); |
---|
636 | |
---|
637 | if ( drvmgr_get_dev(&b1553rt_drv_info.general, minor, &dev) ) { |
---|
638 | return RTEMS_UNSATISFIED; |
---|
639 | } |
---|
640 | rt = (rt_priv *)dev->priv; |
---|
641 | |
---|
642 | if (!ioarg) { |
---|
643 | DBG("rt_control: invalid argument\n"); |
---|
644 | return RTEMS_INVALID_NAME; |
---|
645 | } |
---|
646 | |
---|
647 | ioarg->ioctl_return = 0; |
---|
648 | switch (ioarg->command) { |
---|
649 | |
---|
650 | case RT_SET_ADDR: |
---|
651 | set_rtaddr(rt, data[0]); |
---|
652 | break; |
---|
653 | |
---|
654 | case RT_SET_BCE: |
---|
655 | set_broadcast_en(rt, data[0]); |
---|
656 | break; |
---|
657 | |
---|
658 | case RT_SET_VECTORW: |
---|
659 | set_vector_word(rt, data[0]); |
---|
660 | break; |
---|
661 | |
---|
662 | case RT_SET_EXTMDATA: |
---|
663 | set_extmdata_en(rt, data[0]); |
---|
664 | break; |
---|
665 | |
---|
666 | case RT_RX_BLOCK: |
---|
667 | rt->rx_blocking = data[0]; |
---|
668 | break; |
---|
669 | |
---|
670 | case RT_CLR_STATUS: |
---|
671 | rt->status = 0; |
---|
672 | break; |
---|
673 | |
---|
674 | case RT_GET_STATUS: /* copy status */ |
---|
675 | if ( !ioarg->buffer ) |
---|
676 | return RTEMS_INVALID_NAME; |
---|
677 | |
---|
678 | *(unsigned int *)ioarg->buffer = rt->status; |
---|
679 | break; |
---|
680 | |
---|
681 | case RT_SET_EVENTID: |
---|
682 | rt->event_id = (rtems_id)ioarg->buffer; |
---|
683 | break; |
---|
684 | |
---|
685 | default: |
---|
686 | return RTEMS_NOT_IMPLEMENTED; |
---|
687 | } |
---|
688 | |
---|
689 | return RTEMS_SUCCESSFUL; |
---|
690 | } |
---|
691 | |
---|
692 | static void b1553rt_interrupt(void *arg) |
---|
693 | { |
---|
694 | rt_priv *rt = arg; |
---|
695 | unsigned short descriptor; |
---|
696 | int signal_event=0, wake_rx_task=0; |
---|
697 | unsigned int event_status=0; |
---|
698 | unsigned int wc, irqv, cmd, tsw, suba, tx, miw, i; |
---|
699 | unsigned int ipend; |
---|
700 | |
---|
701 | #define SET_ERROR_DESCRIPTOR(descriptor) (event_status = (event_status & 0x0000ffff) | descriptor<<16) |
---|
702 | ipend = rt->regs->ipm; |
---|
703 | |
---|
704 | if (ipend == 0) { |
---|
705 | /* IRQ mask has been cleared, we must have been reset */ |
---|
706 | /* Restore ctrl registers */ |
---|
707 | rt->regs->ctrl = rt->ctrl_copy; |
---|
708 | rt->regs->addr = rt->memarea_base_remote; |
---|
709 | rt->regs->ipm = 0x70000; |
---|
710 | /* Send reset mode code event */ |
---|
711 | if (rt->head - rt->tail != EVENT_QUEUE_SIZE) { |
---|
712 | miw = (8<<11); |
---|
713 | descriptor = 64 + 32 + 8; |
---|
714 | rt->rt_event[INDEX(rt->head)].miw = miw; |
---|
715 | rt->rt_event[INDEX(rt->head)].time = 0; |
---|
716 | rt->rt_event[INDEX(rt->head)].desc = descriptor; |
---|
717 | rt->head++; |
---|
718 | } |
---|
719 | } |
---|
720 | |
---|
721 | if ( ipend & 0x1 ) { |
---|
722 | /* RT IRQ */ |
---|
723 | if (rt->head - rt->tail != EVENT_QUEUE_SIZE) { |
---|
724 | |
---|
725 | irqv = rt->regs->irq; |
---|
726 | cmd = irqv >> 7; |
---|
727 | wc = cmd & 0x1F; /* word count / mode code */ |
---|
728 | suba = irqv & 0x1F; /* sub address (0-31) */ |
---|
729 | tx = (irqv >> 5) & 1; |
---|
730 | |
---|
731 | /* read status word */ |
---|
732 | tsw = READ_DMA(&rt->mem[tx*0x3E0+suba]); |
---|
733 | |
---|
734 | /* Build Message Information Word (B1553BRM-style) */ |
---|
735 | miw = (wc<<11) | (tsw&RT_TSW_BUS)>>4 | !(tsw&RT_TSW_OK)>>7 | (tsw&RT_TSW_ILL)>>5 | |
---|
736 | (tsw&RT_TSW_PAR)>>5 | (tsw&RT_TSW_MAN)>>7; |
---|
737 | |
---|
738 | descriptor = (tx << 5) | suba; |
---|
739 | |
---|
740 | /* Mode codes */ |
---|
741 | if (suba == 0 || suba == 31) { |
---|
742 | descriptor = 64 + (tx*32) + wc; |
---|
743 | } |
---|
744 | |
---|
745 | /* Data received or transmitted */ |
---|
746 | if (descriptor < 64) { |
---|
747 | wc = wc ? wc : 32; /* wc = 0 means 32 words transmitted */ |
---|
748 | } |
---|
749 | /* RX Mode code */ |
---|
750 | else if (descriptor < 96) { |
---|
751 | wc = (wc>>4); |
---|
752 | } |
---|
753 | /* TX Mode code */ |
---|
754 | else if (descriptor < 128) { |
---|
755 | wc = (wc>>4); |
---|
756 | } |
---|
757 | |
---|
758 | /* Copy to event queue */ |
---|
759 | rt->rt_event[INDEX(rt->head)].miw = miw; |
---|
760 | rt->rt_event[INDEX(rt->head)].time = 0; |
---|
761 | |
---|
762 | for (i = 0; i < wc; i++) { |
---|
763 | rt->rt_event[INDEX(rt->head)].data[i] = READ_DMA(&rt->mem[tx*0x400 + suba*32 + i]); |
---|
764 | } |
---|
765 | rt->rt_event[INDEX(rt->head)].desc = descriptor; |
---|
766 | rt->head++; |
---|
767 | |
---|
768 | |
---|
769 | /* Handle errors */ |
---|
770 | if ( tsw & RT_TSW_ILL){ |
---|
771 | FUNCDBG("RT: RT_ILLCMD\n\r"); |
---|
772 | rt->status |= RT_ILLCMD_IRQ; |
---|
773 | event_status |= RT_ILLCMD_IRQ; |
---|
774 | SET_ERROR_DESCRIPTOR(descriptor); |
---|
775 | signal_event=1; |
---|
776 | } |
---|
777 | |
---|
778 | if ( !(tsw & RT_TSW_OK) ) { |
---|
779 | FUNCDBG("RT: RT_MERR_IRQ\n\r"); |
---|
780 | rt->status |= RT_MERR_IRQ; |
---|
781 | event_status |= RT_MERR_IRQ; |
---|
782 | SET_ERROR_DESCRIPTOR(descriptor); |
---|
783 | signal_event=1; |
---|
784 | } |
---|
785 | |
---|
786 | } |
---|
787 | else { |
---|
788 | /* Indicate overrun */ |
---|
789 | rt->rt_event[INDEX(rt->head)].desc |= 0x8000; |
---|
790 | } |
---|
791 | } |
---|
792 | |
---|
793 | if ( ipend & 0x2 ) { |
---|
794 | /* Memory failure IRQ */ |
---|
795 | FUNCDBG("B1553RT: Memory failure\n"); |
---|
796 | event_status |= RT_DMAF_IRQ; |
---|
797 | signal_event=1; |
---|
798 | } |
---|
799 | |
---|
800 | if ( ipend & 0x4 ) { |
---|
801 | /* AHB Error */ |
---|
802 | FUNCDBG("B1553RT: AHB ERROR\n"); |
---|
803 | event_status |= RT_DMAF_IRQ; |
---|
804 | signal_event=1; |
---|
805 | } |
---|
806 | |
---|
807 | #ifdef DEBUG |
---|
808 | rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = descriptor; |
---|
809 | rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = cmd; |
---|
810 | rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = miw; |
---|
811 | rt->log[rt->log_i++ % EVENT_QUEUE_SIZE] = tsw; |
---|
812 | #endif |
---|
813 | |
---|
814 | wake_rx_task = 1; |
---|
815 | |
---|
816 | /* Wake any blocked rx thread only on receive interrupts */ |
---|
817 | if ( wake_rx_task ) { |
---|
818 | rtems_semaphore_release(rt->rx_sem); |
---|
819 | } |
---|
820 | |
---|
821 | /* Copy current mask to status mask */ |
---|
822 | if ( event_status ) { |
---|
823 | if ( event_status & 0xffff0000 ) |
---|
824 | rt->status &= 0x0000ffff; |
---|
825 | rt->status |= event_status; |
---|
826 | } |
---|
827 | |
---|
828 | /* signal event once */ |
---|
829 | if ( signal_event && (rt->event_id != 0) ) { |
---|
830 | rtems_event_send(rt->event_id, event_status); |
---|
831 | } |
---|
832 | |
---|
833 | } |
---|
834 | |
---|
835 | void b1553rt_print_dev(struct drvmgr_dev *dev, int options) |
---|
836 | { |
---|
837 | rt_priv *pDev = dev->priv; |
---|
838 | |
---|
839 | /* Print */ |
---|
840 | printf("--- B1553RT[%d] %s ---\n", pDev->minor, pDev->devName); |
---|
841 | printf(" REGS: 0x%x\n", (unsigned int)pDev->regs); |
---|
842 | printf(" IRQ: %d\n", pDev->irqno); |
---|
843 | |
---|
844 | } |
---|
845 | |
---|
846 | void b1553rt_print(int options) |
---|
847 | { |
---|
848 | struct amba_drv_info *drv = &b1553rt_drv_info; |
---|
849 | struct drvmgr_dev *dev; |
---|
850 | |
---|
851 | dev = drv->general.dev; |
---|
852 | while(dev) { |
---|
853 | b1553rt_print_dev(dev, options); |
---|
854 | dev = dev->next_in_drv; |
---|
855 | } |
---|
856 | } |
---|