2 * \file udi_lib/physio/pio.c
3 * \author John Hodge (thePowersGang)
7 #include <udi_physio.h>
9 #include <udi_internal.h>
11 typedef void _udi_pio_do_io_op_t(uint32_t child_ID, udi_ubit32_t regset_idx, udi_ubit32_t ofs, size_t len,
12 void *data, bool isOutput);
15 struct udi_pio_handle_s
17 tUDI_DriverInstance *Inst;
18 udi_index_t RegionIdx;
20 _udi_pio_do_io_op_t *IOFunc;
28 udi_pio_trans_t *TransOps;
30 udi_ubit16_t PIOAttributes;
34 // TODO: Cached labels
39 extern _udi_pio_do_io_op_t pci_pio_do_io;
43 EXPORT(udi_pio_unmap);
44 EXPORT(udi_pio_atmic_sizes);
45 EXPORT(udi_pio_abort_sequence);
46 EXPORT(udi_pio_trans);
47 EXPORT(udi_pio_probe);
50 void udi_pio_map(udi_pio_map_call_t *callback, udi_cb_t *gcb,
51 udi_ubit32_t regset_idx, udi_ubit32_t base_offset, udi_ubit32_t length,
52 udi_pio_trans_t *trans_list, udi_ubit16_t list_length,
53 udi_ubit16_t pio_attributes, udi_ubit32_t pace, udi_index_t serialization_domain)
55 LOG("gcb=%p,regset_idx=%i,base_offset=0x%x,length=0x%x,trans_list=%p,list_length=%i,...",
56 gcb, regset_idx, base_offset, length, trans_list, list_length);
57 char bus_type[16] = {0};
58 udi_instance_attr_type_t type;
59 type = udi_instance_attr_get_internal(gcb, "bus_type", 0, bus_type, sizeof(bus_type), NULL);
60 if(type != UDI_ATTR_STRING) {
61 Log_Warning("UDI", "No/invalid bus_type attribute");
62 callback(gcb, UDI_NULL_PIO_HANDLE);
67 _udi_pio_do_io_op_t *io_op;
68 if( strcmp(bus_type, "pci") == 0 ) {
70 io_op = pci_pio_do_io;
74 Log_Warning("UDI", "Unknown bus type %s", bus_type);
75 callback(gcb, UDI_NULL_PIO_HANDLE);
79 udi_pio_handle_t ret = malloc( sizeof(struct udi_pio_handle_s) );
80 ret->Inst = UDI_int_ChannelGetInstance(gcb, false, &ret->RegionIdx);
81 ret->ChildID = ret->Inst->ParentChildBinding->ChildID;
82 ret->RegSet = regset_idx;
84 ret->Offset = base_offset;
86 ret->TransOps = trans_list;
87 // TODO: Pre-scan to get labels
88 ret->nTransOps = list_length;
89 ret->PIOAttributes = pio_attributes;
91 // TODO: Validate serialization_domain
92 ret->Domain = serialization_domain;
97 void udi_pio_unmap(udi_pio_handle_t pio_handle)
102 udi_ubit32_t udi_pio_atmic_sizes(udi_pio_handle_t pio_handle)
108 void udi_pio_abort_sequence(udi_pio_handle_t pio_handle, udi_size_t scratch_requirement)
113 size_t _get_label(udi_pio_handle_t pio_handle, udi_index_t label)
115 udi_pio_trans_t *ops = pio_handle->TransOps;
119 for( ; ip < pio_handle->nTransOps; ip ++ ) {
120 if( ops[ip].pio_op == UDI_PIO_LABEL && ops[ip].operand == label )
128 udi_ubit32_t words[32/4];;
131 void _zero_upper(int sizelog2, _trans_value_t *val)
136 val->words[0] &= 0x00FF;
138 val->words[0] &= 0xFFFF;
154 int _compare_zero(_trans_value_t *val, int sizelog2)
161 is_z = is_z && (val->words[7] == 0);
162 is_z = is_z && (val->words[6] == 0);
163 is_z = is_z && (val->words[5] == 0);
164 is_z = is_z && (val->words[4] == 0);
166 is_z = is_z && (val->words[3] == 0);
167 is_z = is_z && (val->words[2] == 0);
169 is_z = is_z && (val->words[1] == 0);
171 is_z = is_z && (val->words[0] == 0);
172 is_n = (val->words[ (1<<sizelog2)/4-1 ] >> 31) != 0;
175 is_z = (val->words[0] & 0xFFFF) == 0;
176 is_n = (val->words[0] >> 15) != 0;
179 is_z = (val->words[0] & 0xFFFF) == 0;
180 is_n = (val->words[0] >> 7) != 0;
184 return is_z | (is_n << 1);
187 static inline void _operation_shift_left(int sizelog2, _trans_value_t *dstval, int count)
189 const int nwords = (1<<sizelog2)/4;
190 ASSERTC(count, <=, 32);
191 if( sizelog2 <= UDI_PIO_4BYTE ) {
192 dstval->words[0] <<= count;
194 else if( count == 0 ) {
197 else if( count == 32 ) {
198 for(int i = nwords - 1; i --; )
199 dstval->words[i+1] = dstval->words[i];
200 dstval->words[0] = 0;
203 for( int i = nwords - 1; i --; )
204 dstval->words[i+1] = (dstval->words[i+1] << count)
205 | (dstval->words[i] >> (32 - count));
206 dstval->words[0] = dstval->words[0] << count;
208 _zero_upper(sizelog2, dstval);
210 static inline void _operation_shift_right(int sizelog2, _trans_value_t *dstval, int count)
212 const int nwords = (1<<sizelog2)/4;
213 ASSERTC(count, <=, 32);
214 if( sizelog2 <= UDI_PIO_4BYTE ) {
215 dstval->words[0] >>= count;
217 else if( count == 0 ) {
220 else if( count == 32 ) {
221 for(int i = 0; i < nwords-1; i++ )
222 dstval->words[i] = dstval->words[i+1];
223 dstval->words[nwords-1] = 0;
226 for(int i = 0; i < nwords-1; i++ )
227 dstval->words[i] = (dstval->words[i] >> count)
228 | (dstval->words[i+1] << (32 - count));
229 dstval->words[nwords-1] = dstval->words[nwords-1] >> count;
231 _zero_upper(sizelog2, dstval);
233 static inline void _operation_and(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
235 for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
236 dstval->words[i] &= srcval->words[i];
237 _zero_upper(sizelog2, dstval);
239 static inline void _operation_or(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
241 for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
242 dstval->words[i] |= srcval->words[i];
243 _zero_upper(sizelog2, dstval);
245 static inline void _operation_xor(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
247 for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ )
248 dstval->words[i] ^= srcval->words[i];
249 _zero_upper(sizelog2, dstval);
251 static inline void _operation_add(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
254 for( int i = 0; i < ((1 <<sizelog2)+3)/4; i ++ ) {
255 int new_c = (dstval->words[i] + srcval->words[i]) < srcval->words[i];
256 dstval->words[i] += srcval->words[i] + c;
259 _zero_upper(sizelog2, dstval);
261 static inline void _operation_sub(int sizelog2, _trans_value_t *dstval, const _trans_value_t *srcval)
263 if( sizelog2 <= UDI_PIO_4BYTE ) {
264 // Single word doesn't need the borrow logic
265 // - including it would cause bugs
266 dstval->words[0] -= srcval->words[0];
270 for( int i = ((1 <<sizelog2)+3)/4; i --; ) {
271 int new_b = (dstval->words[i] < srcval->words[i] + b);
272 dstval->words[i] -= srcval->words[i] + b;
276 _zero_upper(sizelog2, dstval);
280 static const char *caMEM_MODES[] = {"DIRECT","SCRATCH","BUF","MEM"};
282 static inline int _read_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
283 udi_ubit8_t op, int sizelog2,
284 const _trans_value_t *reg, _trans_value_t *val)
286 udi_ubit32_t ofs = reg->words[0];
287 udi_size_t size = 1 << sizelog2;
291 memcpy(val, reg, size);
292 _zero_upper(sizelog2, val);
294 case UDI_PIO_SCRATCH:
295 ASSERTCR( (ofs & (size-1)), ==, 0, 1);
296 memcpy(val, gcb->scratch + ofs, size);
297 //LOG("scr %p+%i => %i %x,...", gcb->scratch, ofs, size, val->words[0]);
301 udi_buf_read(buf, ofs, size, val);
302 //LOG("buf %p+%i => %i %x,...", buf, ofs, size, val->words[0]);
305 ASSERTCR( (ofs & (size-1)), ==, 0, 1 );
306 memcpy(val, mem_ptr + ofs, size);
307 //LOG("mem %p+%i => %i %x,...", mem_ptr, ofs, size, val->words[0]);
313 static inline int _write_mem(udi_cb_t *gcb, udi_buf_t *buf, void *mem_ptr,
314 udi_ubit8_t op, int sizelog2,
315 _trans_value_t *reg, const _trans_value_t *val)
317 udi_ubit32_t ofs = reg->words[0];
318 udi_size_t size = 1 << sizelog2;
322 //LOG("reg %p = %i %x,...", reg, size, val->words[0]);
323 memcpy(reg, val, size);
324 _zero_upper(sizelog2, reg);
326 case UDI_PIO_SCRATCH:
327 ASSERTCR( (ofs & (size-1)), ==, 0, 1);
328 //LOG("scr %p+%i = %i %x,...", gcb->scratch, ofs, size, val->words[0]);
329 memcpy(gcb->scratch + ofs, val, size);
333 //LOG("buf %p+%i = %i %x,...", buf, ofs, size, val->words[0]);
334 udi_buf_write(NULL,NULL, val, size, buf, ofs, size, UDI_NULL_BUF_PATH);
337 ASSERTCR( (ofs & (size-1)), ==, 0, 1);
338 //LOG("mem %p+%i = %i %x,...", mem_ptr, ofs, size, val->words[0]);
339 memcpy(mem_ptr + ofs, val, size);
345 void udi_pio_trans(udi_pio_trans_call_t *callback, udi_cb_t *gcb,
346 udi_pio_handle_t pio_handle, udi_index_t start_label,
347 udi_buf_t *buf, void *mem_ptr)
349 LOG("pio_handle=%p,start_label=%i,buf=%p,mem_ptr=%p",
350 pio_handle, start_label, buf, mem_ptr);
351 _trans_value_t registers[8];
352 _trans_value_t tmpval;
356 udi_ubit16_t ret_status = 0;
358 udi_pio_trans_t *ops = pio_handle->TransOps;
359 size_t ip = _get_label(pio_handle, start_label);
361 while( ip < pio_handle->nTransOps )
363 udi_ubit8_t pio_op = ops[ip].pio_op;
364 udi_ubit8_t tran_size = ops[ip].tran_size;
365 udi_ubit16_t operand = ops[ip].operand;
366 LOG("%2i: %02x %i 0x%04x", ip, pio_op, tran_size, operand);
367 ASSERTC(tran_size, <=, UDI_PIO_32BYTE);
369 if( !(pio_op & 0x80) )
372 _trans_value_t *reg = ®isters[pio_op&7];
373 switch(pio_op & 0x60)
376 pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
377 operand, tran_size, &tmpval, false);
378 LOG("IN %x = %i %x", operand, tran_size, tmpval.words[0]);
379 _write_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, &tmpval);
382 _read_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, &tmpval);
383 LOG("OUT %x = %i %x", operand, tran_size, tmpval.words[0]);
384 pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
385 operand, tran_size, &tmpval, true);
388 _read_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, ®isters[operand]);
389 LOG("LOAD R%x = %i %x", operand, tran_size, registers[operand].words[0]);
390 _zero_upper(tran_size, ®isters[operand]);
393 LOG("STORE R%x (%i %x)", operand, tran_size, registers[operand].words[0]);
394 _write_mem(gcb, buf, mem_ptr, (pio_op&0x18), tran_size, reg, ®isters[operand]);
398 else if( pio_op < 0xF0 )
400 int reg_num = pio_op & 7;
401 _trans_value_t *reg = ®isters[reg_num];
403 switch(pio_op & 0xF8)
405 case UDI_PIO_LOAD_IMM:
406 ASSERTC(ip + (1<<tran_size)/2-1, <=, pio_handle->nTransOps);
407 if( tran_size == UDI_PIO_1BYTE ) {
408 Log_Error("UDI", "udi_pio_trans - %p [%i] LOAD_IMM with 1 byte",
415 reg->words[7] = (ops[ip+14].operand << 16) | ops[ip+13].operand;
416 reg->words[6] = (ops[ip+12].operand << 16) | ops[ip+11].operand;
417 reg->words[5] = (ops[ip+10].operand << 16) | ops[ip+ 9].operand;
418 reg->words[4] = (ops[ip+ 8].operand << 16) | ops[ip+ 7].operand;
420 reg->words[3] = (ops[ip+ 6].operand << 16) | ops[ip+ 5].operand;
421 reg->words[2] = (ops[ip+ 4].operand << 16) | ops[ip+ 3].operand;
423 reg->words[1] = (ops[ip+ 2].operand << 16) | ops[ip+ 1].operand;
425 reg->words[0] = (ops[ip+ 0].operand << 16) | operand;
428 reg->words[0] = operand;
432 _zero_upper(tran_size, reg);
433 ip += (1<<tran_size)/2-1;
435 case UDI_PIO_CSKIP: {
436 int cnd = _compare_zero(reg, tran_size);
440 LOG("CSKIP NZ R%i (%i %x)", reg_num, tran_size, reg->words[0]);
445 LOG("CSKIP Z R%i (%i %x)", reg_num, tran_size, reg->words[0]);
450 LOG("CSKIP NNEG R%i (%i %x)", reg_num, tran_size, reg->words[0]);
455 LOG("CSKIP NEG R%i (%i %x)", reg_num, tran_size, reg->words[0]);
463 pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
464 registers[operand].words[0], tran_size, reg, false);
465 _zero_upper(tran_size, reg);
467 case UDI_PIO_OUT_IND:
469 pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
470 registers[operand].words[0], tran_size, reg, true);
472 case UDI_PIO_SHIFT_LEFT:
473 _operation_shift_left(tran_size, reg, operand);
475 case UDI_PIO_SHIFT_RIGHT:
476 _operation_shift_right(tran_size, reg, operand);
479 _operation_and(tran_size, reg, ®isters[operand]);
481 case UDI_PIO_AND_IMM:
482 LOG("AND_IMM R%i &= 0x%x", reg_num, operand);
483 tmpval.words[0] = operand;
484 _zero_upper(UDI_PIO_2BYTE, &tmpval);
485 _operation_and(tran_size, reg, &tmpval);
488 _operation_or(tran_size, reg, ®isters[operand]);
491 LOG("OR_IMM R%i |= 0x%x", reg_num, operand);
492 tmpval.words[0] = operand;
493 _zero_upper(UDI_PIO_4BYTE, &tmpval);
494 _operation_or(tran_size, reg, &tmpval);
497 _operation_xor(tran_size, reg, ®isters[operand]);
500 LOG("ADD R%i += R%i", reg_num, operand);
501 ASSERTC(operand, <, 8);
502 _operation_add(tran_size, reg, ®isters[operand]);
504 case UDI_PIO_ADD_IMM:
505 tmpval.words[0] = operand;
506 if( operand & (1 <<16) ) {
507 tmpval.words[0] |= 0xFFFF0000;
508 memset(&tmpval.words[1], 0xFF, 4*(8-1));
511 _zero_upper(UDI_PIO_4BYTE, &tmpval);
513 LOG("ADD R%i += 0x%x", reg_num, tmpval.words[0]);
514 _operation_add(tran_size, reg, &tmpval);
517 LOG("SUB R%i -= R%i", reg_num, operand);
518 ASSERTC(operand, <, 8);
519 _operation_sub(tran_size, reg, ®isters[operand]);
522 Log_Error("UDI", "udi_pio_trans - Class B %x unhandled!",
533 LOG("BRANCH %i", operand);
534 ip = _get_label(pio_handle, operand);
538 LOG("LABEL %i", operand);
540 case UDI_PIO_REP_IN_IND:
541 case UDI_PIO_REP_OUT_IND: {
542 bool dir_is_out = (pio_op == UDI_PIO_REP_OUT_IND);
543 udi_ubit8_t mode = operand & 0x18;
544 udi_ubit32_t cnt = registers[(operand>>13) & 7].words[0];
545 int pio_stride = (operand>>10) & 3;
546 udi_ubit32_t pio_ofs = registers[(operand>>7) & 7].words[0];
547 int mem_stride = (operand>>5) & 3;
548 _trans_value_t *mem_reg=®isters[(operand>>0) & 7];
549 udi_ubit32_t saved_mem_reg = mem_reg->words[0];
552 pio_stride = 1<<(tran_size+pio_stride-1);
554 mem_stride = 1<<(tran_size+mem_stride-1);
556 LOG("REP_%s_IND %i IO=%x+%i %s Mem=%x+%i",
557 (dir_is_out ? "OUT": "IN"), cnt,
559 caMEM_MODES[mode>>3], saved_mem_reg, mem_stride
565 _read_mem(gcb,buf,mem_ptr, mode, tran_size,
567 pio_handle->IOFunc(pio_handle->ChildID, pio_handle->RegSet,
568 pio_ofs, tran_size, &tmpval, dir_is_out);
570 _write_mem(gcb,buf,mem_ptr, mode, tran_size,
572 pio_ofs += pio_stride;
573 if( mode != UDI_PIO_DIRECT )
574 mem_reg->words[0] += mem_stride;
576 if( mode != UDI_PIO_DIRECT )
577 mem_reg->words[0] = saved_mem_reg;
581 LOG("DELAY %i", operand);
582 Log_Notice("UDI", "udi_pio_trans - TODO: DELAY");
584 case UDI_PIO_BARRIER:
586 Log_Notice("UDI", "udi_pio_trans - TODO: BARRIER");
590 Log_Notice("UDI", "udi_pio_trans - TODO: SYNC");
592 case UDI_PIO_SYNC_OUT:
594 Log_Notice("UDI", "udi_pio_trans - TODO: SYNC_OUT");
597 LOG("DEBUG %x", operand);
601 ASSERTC(operand, <, 8);
602 ASSERTC(tran_size, <=, UDI_PIO_2BYTE);
603 if( tran_size == UDI_PIO_2BYTE )
604 ret_status = registers[operand].words[0] & 0xFFFF;
606 ret_status = registers[operand].words[0] & 0xFF;
607 LOG("END R%i 0x%x", operand, ret_status);
609 case UDI_PIO_END_IMM:
610 LOG("END IMM 0x%x", operand);
611 ASSERTC(tran_size, ==, UDI_PIO_2BYTE);
612 ret_status = operand;
615 Log_Error("UDI", "udi_pio_trans - Class C %x unimplemented",
622 if( ip == pio_handle->nTransOps ) {
623 Log_Notice("UDI", "udi_pio_trans - %p: Overran transaction list",
627 callback(gcb, buf, UDI_OK, ret_status);
630 callback(gcb, buf, UDI_STAT_HW_PROBLEM, 0);
633 void udi_pio_probe(udi_pio_probe_call_t *callback, udi_cb_t *gcb,
634 udi_pio_handle_t pio_handle, void *mem_ptr, udi_ubit32_t pio_offset,
635 udi_ubit8_t tran_size, udi_ubit8_t direction)